无人机图片目标地理位置计算

已于 2025-04-02 10:57:54 修改 · 851 阅读

文章标签：

于 2025-04-02 10:09:28 首次发布

因为项目需要，需要通过无人机视频的中的目标位置计算实际的地理坐标位置，一下是计算的方法记录。

1、坐标系定义：

像素坐标系：单位pixel，相机的成像平面，原点在图像的左上方，u轴向右，v轴向下，像素坐标系的单位是像素(pixel)，也就是分辨率。
图像坐标系：单位mm，和像素坐标系在同一个平面上，原点是相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫principal point。单位为物理单位。

机体坐标系：单位m，是以载机位置为原点建立的直角坐标系，X轴指向机头方向，Z轴指向载机垂直向下。即带姿态。
地理坐标系：单位m，以载机位置为原点建立的NED北东地坐标系
大地直角坐标系：单位m，根据参考椭球面建立的笛卡尔直角坐标系，原点为参考椭球面的中心点，Z轴由原点指向地球北极; X轴由原点指向本初子午圈与赤道圈在椭球面上的交点;
WGS84坐标系：与大地直角坐标系一样，只是采用纬度(M)、经度(L)和大地高(H)表示空间中任一点位置。

相机坐标系：单位m，原点是光心，x和y轴与像素坐标系u轴和v轴平行，z轴为相机的光轴。光心到像素平面的距离为焦距f。相机坐标系上的点和成像平面坐标系上的点存在透视投影关系

参考文献：【教程】详解相机模型与坐标转换-CSDN博客

2、像素坐标系转图像坐标系

从无人机所拍摄的视频图片的像素坐标系转图片坐标系

(u,v) 目标点的像素坐标系中的坐标
(u0,v0) 像素坐标系的中心点
(dx,dy) 每个像素代表在图像坐标系中的距离单位mm
(imageX,imageY) 图像的尺寸单位像素
(rx,ry) 目标点在图像中的位置（范围0~1）的比值

def pixel_to_image(u,u0,v,v0,dx,dy):
    x=(u-u0)*dx
    y=(v-v0)*dy
    return [x,y]

def getPixel(imageX,imageY,rx,ry):
    return imageX*rx,imageY*ry,imageX/2,imageY/2

u,v,u0,v0=getPixel(imageX,imageY,rx,ry)
print(u,v,u0,v0)
x,y=pixel_to_image(u,u0,v,v0,dx,dy)
print(x,y)

3、图像坐标系转机体坐标系

(x,y) 目标点在图像坐标系中的坐标
H 目标点在机体坐标系中的Z坐标，为相机到目标点实际的高度距离，在这里我们可以使用无人机飞行的相对高度代替（即默认为所有的目标点的海拔高度与无人机起飞的高度一致，如果目标点区域为高低起伏较大的区域，则需要通过dem来缩小误差）
f 相机的实际焦距（注意，并非相机厂商提供的等效焦距），即相机到成片的距离

def image_to_drone(x,y,H,f):
    Zc=H
    Xc=Zc/f*x
    Yc=Zc/f*y
    return [Xc,Yc,Zc]

Xc,Yc,Zc=image_to_drone(x,y,rel_H,f)
print(Xc,Yc,Zc)

4、机体坐标系转 neu地理坐标系

γ 横滚：北偏东为正
β 俯仰：抬头为正
α 航向：右倾斜为正
(x,y,z) 目标点在机体坐标系中的位置

注意此处计算结果为neu坐标系，需要交换结果的前两个参数转为enu坐标系

def drone_to_neu(α,β,γ,x,y,z):
    α = np.radians(α) 
    β = np.radians(β)
    γ = np.radians(γ)
    C11=np.cos(α)*np.cos(β)
    C12=np.cos(α)*np.sin(β)*np.sin(γ)-np.sin(α)*np.cos(γ)
    C13=-np.cos(α)*np.sin(β)*np.cos(γ)-np.sin(α)*np.sin(γ)
    C21=np.sin(α)*np.cos(β)
    C22=np.cos(α)*np.cos(γ)+np.sin(α)*np.sin(β)*np.sin(γ)
    C23=-np.sin(α)*np.sin(β)*np.cos(γ)+np.cos(α)*np.sin(γ)
    C31=np.sin(β)
    C32=-np.cos(β)*np.sin(γ)
    C33=np.cos(β)*np.cos(γ)
    Ccb=np.array([[C11,C12,C13],[C21,C22,C23],[C31,C32,C33]])
    xyz=np.array([x,y,z])
    return np.dot(Ccb,xyz)

5、enu坐标系转大地直角坐标系

(lon,lat,H) 无人机的经纬高度
(x,y,z) 目标点在enu坐标系中的位置

此处的H为无人机的绝对高度，即无人机的海拔高度，与上面的相对高度（无人机起飞的高度）不同。

import pymap3d as pm

def enu_to_ecef(xb,yb,zb,lon,lat,H):
    return pm.enu2ecef(xb,yb,zb,lat,lon,H)

6、大地直角坐标系：ECEF (EPSG:4978) 转 WGS84 经纬高 (EPSG:4979)

# 定义坐标系：ECEF (EPSG:4978) 转 WGS84 经纬高 (EPSG:4979)
"""
    使用 pyproj 将 ECEF 坐标转换为经纬高 (WGS84)
    参数: X,Y,Z (单位: 米)
    返回: 经度lon(度), 纬度lat(度), 高度h(米)
"""

from pyproj import Proj
from pyproj import Transformer

def ecef_to_lla(X,Y,Z):
    transformer = Transformer.from_crs("EPSG:4978", "EPSG:4979", always_xy=True)
    lon, lat, h = transformer.transform(X,Y,Z)
    return lon, lat, h

7、相机物理焦距的计算方法

import math

def calculate_physical_focal_length(equivalent_focal_length, sensor_width, sensor_height):
    # 计算传感器的对角线长度 (mm)
    sensor_diagonal = math.sqrt(sensor_width**2 + sensor_height**2)
    
    # 计算裁切系数（Crop Factor）
    crop_factor = 43.3 / sensor_diagonal  # 43.3 是 35mm 全画幅传感器的对角线长度 (mm)
    
    # 计算物理焦距 (mm)
    physical_focal_length = equivalent_focal_length / crop_factor
    
    return physical_focal_length, sensor_diagonal, crop_factor

# 输入参数
equivalent_focal_length = 24  # 等效焦距 (mm)
sensor_width = 9.69  # 传感器宽度 (mm)
sensor_height = 7.27  # 传感器高度 (mm)

# 计算
physical_focal_length, sensor_diagonal, crop_factor = calculate_physical_focal_length(
    equivalent_focal_length, sensor_width, sensor_height
)

# 输出结果
print(f"传感器对角线: {sensor_diagonal:.2f} mm")
print(f"裁切系数: {crop_factor:.2f}")
print(f"物理焦距: {physical_focal_length:.2f} mm")