无人机视觉：连接像素与现实世界 —— 像素与GPS坐标双向转换指南

在无人机航拍应用中，一个核心的需求是将图像上的某个点与现实世界中的地理位置精确对应起来。无论是目标跟踪、地图测绘还是农情监测，理解图像像素与其对应的经纬度（GPS坐标）之间的关系至关重要。本文将详细介绍如何实现单个像素坐标到GPS坐标的双向转换，并提供基于Python的实现思路。

核心问题

1. 像素坐标 -> GPS坐标： 已知图像上一个点的像素坐标 (u, v)，以及拍摄时无人机的状态（位置、姿态、相机参数），如何计算该像素点对应的地面GPS经纬度 (Lon, Lat)？
2. GPS坐标 -> 像素坐标： 已知地面上一个目标的GPS经纬度 (Lon, Lat)，以及拍摄时无人机的状态，如何反算出该目标出现在图像中的像素坐标 (u, v)？

所需信息（前提条件）

• 图像参数： 图像宽度 img_width (像素)，图像高度 img_height (像素)。
• 相机内参：
  • 传感器尺寸：宽度 sensor_width (毫米)，高度 sensor_height (毫米)。
  • 相机焦距 focal_length (毫米)。
• 无人机状态信息：
  • 相机位置：经度 camera_lon (度)，纬度 camera_lat (度)，相对于海平面的高度或起飞点的高度 flight_height (米)。
  • 相机姿态：偏航角 yaw_angle (度)，俯仰角 pitch_angle (度)，滚转角 roll_angle (度)。

坐标系约定（非常重要！）

为了保证计算的正确性，我们必须明确并统一坐标系的定义。以下是一种常见的约定，你的具体实现可能需要根据实际情况调整：

1. 图像坐标系 (u, v)：
   • 原点：图像左上角。
   • u轴：水平向右。
   • v轴：垂直向下。
   • 单位：像素。
2. 相机坐标系 (Xc, Yc, Zc)：
   • 原点：相机光心。
   • Xc轴：指向相机传感器的右方。
   • Yc轴：指向相机传感器的下方（与图像v轴同向）。
   • Zc轴：指向相机前方（光轴方向）。
   • 单位：米。
   • 注意：也有约定Yc轴向上的，这会影响内参矩阵 fy 和后续计算中的符号。本文档和示例代码采用Yc向下的约定。
3. 世界/导航坐标系 (Xw, Yw, Zw)：
   • 通常使用 NED (North-East-Down) 坐标系。
   • 原点：无人机（相机）位置在地面上的投影点。
   • Xw轴：指向正北 (North)。
   • Yw轴：指向正东 (East)。
   • Zw轴：指向地心 (Down)。
   • 单位：米。
4. 地理坐标系 (Lat, Lon)：
   • 使用WGS84标准。
   • 单位：度。

第一部分：像素坐标 (u, v) -> GPS坐标 (Lon, Lat)

这个过程模拟了光线从地面点穿过相机光心最终成像在传感器像素上的逆过程。
步骤：

1. 计算相机内参矩阵 (K)：

• 将焦距从毫米转换为米：focal_m = focal_length / 1000。

• 将传感器尺寸从毫米转换为米：

  • sensor_width_m = sensor_width / 1000,
  • sensor_height_m = sensor_height / 1000。

• 计算以像素为单位的焦距：

  • fx = focal_m / sensor_width_m * img_width
  • fy = focal_m / sensor_height_m * img_height

• 计算主点（通常是图像中心）：

  • cx = img_width / 2
  • cy = img_height / 2

• 构建内参矩阵 K:

  K = np.array([[fx, 0, cx],[0, fy, cy],[0, 0,  1]
  ])
  

• 计算 K 的逆矩阵 K_inv = np.linalg.inv(K)。

2. 像素坐标转相机坐标系方向向量：

• 将像素坐标 (u, v) 转换为齐次坐标 [u, v, 1]。
• 使用内参矩阵的逆 K_inv 将像素坐标转换到相机坐标系下的一个方向向量：
  • camera_coords_homogeneous = K_inv @ np.array([u, v, 1])
• 这个向量表示从相机光心指向该像素方向的三维射线。为了后续计算，我们通常将其归一化或设定一个深度。由于我们采用 Zc 轴向前的约定，可以将 Zc 分量设为 1（或者 -1，取决于后续旋转定义，这里设为 1 方便理解）：
  • Xc = camera_coords_homogeneous[0]
  • Yc = camera_coords_homogeneous[1]
  • Zc = 1.0 # 方向向量，Z分量设为1

3. 计算旋转矩阵 ( R )：

   • 将无人机姿态角（Yaw, Pitch, Roll）从度转换为弧度。

   • 根据 世界坐标系 (NED) 到 相机坐标系 的转换关系，构建旋转矩阵。这通常是一个组合旋转，例如按 ZYX 的顺序（先绕Z轴偏航，再绕新的Y轴俯仰，最后绕新的X轴滚转 - 这是内旋顺序；或者按 ZYX 外旋，先滚转，再俯仰，最后偏航）。一个常见的 NED 到 相机 (X右Y下Z前) 的旋转矩阵构建方式（外旋 ZYX，对应 Roll, Pitch, Yaw）：

     # 角度转弧度
     yaw_rad, pitch_rad, roll_rad = np.radians(yaw_angle), np.radians(pitch_angle), np.radians(roll_angle)# 基本旋转矩阵
     Rx = np.array([[1, 0, 0],[0, np.cos(roll_rad), -np.sin(roll_rad)],[0, np.sin(roll_rad), np.cos(roll_rad)]]) # Roll 绕 X (North)Ry = np.array([[np.cos(pitch_rad), 0, np.sin(pitch_rad)],[0, 1, 0],[-np.sin(pitch_rad), 0, np.cos(pitch_rad)]]) # Pitch 绕 Y (East)Rz = np.array([[np.cos(yaw_rad), -np.sin(yaw_rad), 0],[np.sin(yaw_rad), np.cos(yaw_rad), 0],[0, 0, 1]]) # Yaw 绕 Z (Down)# 从 NED 到 机体坐标系的旋转矩阵 (注意顺序和内外旋定义)
     # 如果姿态角是相对于NED的，常见的组合是 R = Rz @ Ry @ Rx
     # 这个 R 表示将 NED 坐标系下的向量转换到相机坐标系下
     # 我们需要的是从相机坐标系转到NED，所以用 R 的转置 R.T
     R_cam_to_ned = (Rz @ Ry @ Rx).T
     

     - 注意：旋转矩阵的构建方式和顺序严格依赖于姿态角的定义（是相对于NED还是机体？旋转顺序是怎样的？）。上述是一个示例，你需要根据你的无人机数据定义来确定正确的 R。

4. 将相机坐标系方向向量旋转到世界坐标系 (NED)：

   • world_direction = R_cam_to_ned @ np.array([Xc, Yc, Zc])

5. 计算射线与地面的交点：

   • 假设地面是平坦的，位于无人机下方 flight_height 米处。
   • 在NED坐标系中，地面点的 Zw 坐标为 flight_height。
   • 我们需要找到缩放因子 scale，使得 scale * world_direction 的 Z 分量等于 flight_height。
   • scale = flight_height / world_direction[2] (Zw 分量)
   • 检查：如果 world_direction[2] <= 0，表示射线指向天空或水平，无法与下方地面相交，应返回无效值。
   • 计算地面点在NED坐标系下的坐标（相对于无人机投影点）：
     • ground_ned_x = scale * world_direction[0] (North)
     • ground_ned_y = scale * world_direction[1] (East)

6. 将NED坐标偏移转换为经纬度偏移：

   • 计算相机所在纬度的地球半径 R_earth (可以使用 WGS84 模型精确计算，或用平均半径近似)。
   • 纬度变化 delta_lat = (ground_ned_x / R_earth) * (180 / np.pi) (向北为正)
   • 经度变化 delta_lon = (ground_ned_y / (R_earth * np.cos(np.radians(camera_lat)))) * (180 / np.pi) (向东为正)

7. 计算最终GPS坐标：

   • target_lat = camera_lat + delta_lat
   • target_lon = camera_lon + delta_lon

第二部分：GPS坐标 (Lon, Lat) -> 像素坐标 (u, v)

这个过程模拟光线从目标发出，经过空间变换，最终投影到相机传感器的过程。
步骤：

1. 计算目标点相对于无人机投影点的NED坐标：

   • 计算目标点与相机之间的经纬度差值：
     • delta_lat_deg = target_lat - camera_lat
     • delta_lon_deg = target_lon - camera_lon
   • 计算相机所在纬度的地球半径 R_earth。
   • 将经纬度差值转换为NED坐标系下的米：
     • ground_ned_x = delta_lat_deg * (np.pi / 180) * R_earth (North)
     • ground_ned_y = delta_lon_deg * (np.pi / 180) * (R_earth * np.cos(np.radians(camera_lat))) (East)
   • 目标点在NED坐标系下的向量为 [ground_ned_x, ground_ned_y, flight_height] (因为 Zw 指向下方)。

2. 计算从世界坐标系 (NED) 到相机坐标系的旋转矩阵 (R_ned_to_cam)：

   • 这通常是第一部分中 R_cam_to_ned 的逆矩阵，也就是它的转置 R_ned_to_cam = R_cam_to_ned.T。使用与第一部分相同的姿态角和旋转顺序构建。
   • R_ned_to_cam = Rz @ Ry @ Rx (假设使用 ZYX 外旋)

3. 将目标点的NED坐标转换到相机坐标系：

   • target_cam_coords = R_ned_to_cam @ np.array([ground_ned_x, ground_ned_y, flight_height])
   • 得到目标点在相机坐标系下的 (Xc, Yc, Zc)。

4. 计算相机内参矩阵 (K)：

   • 同第一部分的步骤1。
   • 注意符号问题： 如果你的相机坐标系定义是 Yc 向上，而图像坐标系 v 向下，则 K 矩阵中的 fy 可能需要取负值，或者在投影计算时调整。如果像我们约定的 Yc 向下，则 K 不需要修改。

     # 假设 K 与第一部分相同 (Yc向下)
     K = np.array([[fx, 0, cx],[0, fy, cy],[0, 0,  1]
     ])
     

5. 将相机坐标系下的点投影到像素坐标系：

   • 应用内参矩阵 K：pixel_homogeneous = K @ target_cam_coords
   • 进行透视除法（齐次坐标转普通坐标）：
     • u_raw = pixel_homogeneous[0] / pixel_homogeneous[2]
     • v_raw = pixel_homogeneous[1] / pixel_homogeneous[2]

6. 得到最终像素坐标：

   • u = int(round(u_raw))
   • v = int(round(v_raw))
   • 检查：计算出的 (u, v) 是否在图像边界内 (0 <= u < img_width, 0 <= v < img_height)？如果不在，说明该GPS点不在当前图像视野内。

Python 代码示例

下面是实现像素坐标与GPS坐标双向转换的完整Python代码：

import numpy as npclass UAVGeolocationCalculator:def __init__(self, img_width, img_height, sensor_width, sensor_height):"""初始化计算器。Args:img_width: 图像宽度（单位：像素）。img_height: 图像高度（单位：像素）。sensor_width: 传感器的宽度（单位：毫米）。sensor_height: 传感器的高度（单位：毫米）。"""self.sensor_width = sensor_width / 1000  # 转换为米self.sensor_height = sensor_height / 1000  # 转换为米self.img_width = img_widthself.img_height = img_heightdef _calculate_earth_radius_wgs84(self, latitude):"""计算给定纬度的地球半径"""a = 6378137.0  # WGS84 赤道半径f = 1 / 298.257223563  # WGS84 扁率b = a * (1 - f)  # WGS84 极半径lat_rad = np.radians(latitude)R = np.sqrt((a**2 * np.cos(lat_rad))**2 + (b**2 * np.sin(lat_rad))**2) / \np.sqrt((a * np.cos(lat_rad))**2 + (b * np.sin(lat_rad))**2)return Rdef _calculate_internal_parameter_matrix(self, focal_length, apply_coordinate_transform=False):"""计算相机的内参矩阵。Args:focal_length (float): 焦距，单位为米。apply_coordinate_transform (bool): 是否应用坐标系转换。当为True时，使用带负号的形式。Returns:np.ndarray: 内参矩阵。"""# 焦距转换为像素单位fx = focal_length / self.sensor_width * self.img_widthfy = focal_length / self.sensor_height * self.img_heightcx = self.img_width / 2cy = self.img_height / 2if apply_coordinate_transform:  # 使用坐标系转换来处理相机和图像坐标系差异K = np.array([[-fx, 0, cx],[0, -fy, cy],[0, 0, 1]])else:  # 不使用坐标系转换K = np.array([[fx, 0, cx],[0, fy, cy],[0, 0, 1]])return Kdef _calculate_rotation_matrix(self, yaw, pitch, roll):"""计算旋转矩阵。Args:yaw: 偏航角pitch: 俯仰角roll: 翻滚角Returns:R: 旋转矩阵"""yaw_rad, pitch_rad, roll_rad = np.radians(yaw), np.radians(pitch), np.radians(roll)# 基本旋转矩阵Rz = np.array([[np.cos(yaw_rad), -np.sin(yaw_rad), 0],[np.sin(yaw_rad), np.cos(yaw_rad), 0],[0, 0, 1]])Ry = np.array([[np.cos(pitch_rad), 0, np.sin(pitch_rad)],[0, 1, 0],[-np.sin(pitch_rad), 0, np.cos(pitch_rad)]])Rx = np.array([[1, 0, 0],[0, np.cos(roll_rad), -np.sin(roll_rad)],[0, np.sin(roll_rad), np.cos(roll_rad)]])# 注意旋转顺序，这里采用ZYX外旋R = Rz @ Ry @ Rxreturn Rdef pixel_to_gps(self, pixel_x, pixel_y, camera_lat, camera_lon, flight_height, focal_length,yaw_angle=0.0, pitch_angle=0.0, roll_angle=0.0):"""根据无人机相机的参数和位置，计算图像指定像素点的经纬度。Args:pixel_x (int): 目标像素点的横坐标。pixel_y (int): 目标像素点的纵坐标。camera_lat (float): 相机所在的纬度（度）。camera_lon (float): 相机所在的经度（度）。flight_height (float): 飞行高度（米）。focal_length (float): 相机焦距（毫米）。yaw_angle (float): 相机的偏航角（度），默认值为0.0。pitch_angle (float): 相机的俯仰角（度），默认值为0.0。roll_angle (float): 相机的翻滚角（度），默认值为0.0。Returns:tuple: 包含两个元素(lon, lat)的元组，分别表示指定像素点的经度和纬度。"""# 像素点到相机中心的三维坐标转换K_inv = np.linalg.inv(self._calculate_internal_parameter_matrix(focal_length / 1000))camera_coords = K_inv @ np.array([pixel_x, pixel_y, 1])# 设置射线方向，通常将Z方向设为向前(1)或向后(-1)# 具体取值依赖于你后续的计算约定px_x, px_y, px_z = camera_coords[0], camera_coords[1], -1  # 计算旋转矩阵R = self._calculate_rotation_matrix(yaw_angle, pitch_angle, roll_angle)# 应用旋转矩阵，从相机坐标系转到世界坐标系(NED)R_cam_to_ned = R.T  # 从相机到NED需要转置rotated_coords = R_cam_to_ned @ np.array([px_x, px_y, px_z])# 检查是否指向天空if rotated_coords[2] >= 0:  # Z轴向下，小于0才能与地面相交return np.nan, np.nan# 计算与地面的交点scale = flight_height / -rotated_coords[2]  # 使用Z坐标计算缩放比例ground_x = rotated_coords[0] * scale  # Northground_y = rotated_coords[1] * scale  # East# 地球半径R_earth = self._calculate_earth_radius_wgs84(camera_lat)# 计算经纬度偏移delta_lat = (ground_x / R_earth) * (180 / np.pi)delta_lon = (ground_y / (R_earth * np.cos(np.radians(camera_lat)))) * (180 / np.pi)# 计算最终经纬度lat = camera_lat + delta_latlon = camera_lon + delta_lonreturn lon, latdef gps_to_pixel(self, target_lat, target_lon, camera_lat, camera_lon, flight_height, focal_length,yaw_angle=0.0, pitch_angle=0.0, roll_angle=0.0):"""根据无人机相机的参数和位置，计算指定经纬度在图像中的坐标。Args:target_lat (float): 目标点的纬度（度）。target_lon (float): 目标点的经度（度）。camera_lat (float): 相机所在的纬度（度）。camera_lon (float): 相机所在的经度（度）。flight_height (float): 飞行高度（米）。focal_length (float): 相机焦距（毫米）。yaw_angle (float): 相机的偏航角（度），默认值为0.0。pitch_angle (float): 相机的俯仰角（度），默认值为0.0。roll_angle (float): 相机的翻滚角（度），默认值为0.0。Returns:tuple: 包含两个元素(x, y)的元组，分别表示指定经纬度点在图像上的像素坐标。"""# 地球半径R_earth = self._calculate_earth_radius_wgs84(camera_lat)# 计算经纬度差值delta_lat = target_lat - camera_latdelta_lon = target_lon - camera_lon# 将经纬度差值转换为地面坐标系中的米ground_x = delta_lat * (R_earth) * (np.pi / 180)  # Northground_y = delta_lon * (R_earth * np.cos(np.radians(camera_lat))) * (np.pi / 180)  # East# 计算旋转矩阵R = self._calculate_rotation_matrix(yaw_angle, pitch_angle, roll_angle)# 将地面坐标转换为相机坐标系中的坐标# 注意Z坐标是flight_height，因为NED坐标系Z轴向下camera_coords = R @ np.array([ground_x, ground_y, -flight_height])# 使用内参矩阵计算像素坐标# 这里使用apply_coordinate_transform=True来处理坐标系差异K = self._calculate_internal_parameter_matrix(focal_length / 1000, apply_coordinate_transform=True)# 检查点是否在相机前方if camera_coords[2] <= 0:return None, None# 投影到像素坐标系px = K @ camera_coordspx_x = px[0] / px[2]px_y = px[1] / px[2]# 检查是否在图像内if 0 <= px_x < self.img_width and 0 <= px_y < self.img_height:return int(round(px_x)), int(round(px_y))else:return None, None

使用示例

if __name__ == '__main__':# 创建计算器实例img_width = 1920img_height = 1080sensor_width = 7.53   # 传感器宽度(毫米)sensor_height = 5.64  # 传感器高度(毫米)calculator = UAVGeolocationCalculator(img_width, img_height, sensor_width, sensor_height)# 设置无人机和相机参数focal_length = 12.0   # 相机焦距(毫米)camera_lon = 116.3972   # 拍摄位置经度camera_lat = 39.9075    # 拍摄位置纬度flight_height = 100.0   # 飞行高度(米)yaw_angle = 45.0        # 相机偏航角(度)pitch_angle = -30.0     # 相机俯仰角(度)roll_angle = 0.0        # 相机翻滚角(度)# 测试像素坐标到GPS坐标的转换pixel_x, pixel_y = 960, 540  # 图像中心点lon, lat = calculator.pixel_to_gps(pixel_x, pixel_y, camera_lat, camera_lon, flight_height, focal_length, yaw_angle, pitch_angle, roll_angle)print(f"像素坐标 ({pixel_x}, {pixel_y}) 对应的GPS坐标为: 经度={lon:.7f}, 纬度={lat:.7f}")# 测试GPS坐标到像素坐标的转换pixel_x_back, pixel_y_back = calculator.gps_to_pixel(lat, lon, camera_lat, camera_lon, flight_height,focal_length, yaw_angle, pitch_angle, roll_angle)print(f"GPS坐标 (经度={lon:.7f}, 纬度={lat:.7f}) 对应的像素坐标为: ({pixel_x_back}, {pixel_y_back})")

重要考虑与局限性

1. 坐标系一致性： 这是最容易出错的地方。务必确保你使用的无人机姿态角（Yaw, Pitch, Roll）的定义、旋转顺序与代码中构建旋转矩阵的方式完全匹配。传感器输出的坐标系、相机安装的相对姿态都需要考虑。

2. 传感器精度： 转换结果的精度直接受限于GPS定位精度、IMU姿态测量精度以及相机内参标定的准确性。

3. 地球模型： 上述计算使用了WGS84椭球模型的一个半径近似值。对于高精度要求，应使用更精确的地理坐标转换库（如 pyproj）。

4. 平坦地面假设： 计算假设目标点所在的地面是平坦的，并且位于无人机下方 flight_height 处。对于地形起伏较大的区域，需要引入数字高程模型 (DEM) 数据来获取目标点的实际海拔，从而修正 flight_height 或直接计算射线与地形的交点。

5. 镜头畸变： 未考虑相机镜头的畸变。对于广角镜头或精度要求高的场景，需要先对像素坐标进行去畸变处理，或在投影模型中加入畸变参数。

结论

实现像素坐标与GPS坐标的准确双向转换是无人机数据处理中的一项基本而关键的技术。理解背后的坐标系变换、相机模型和旋转矩阵是核心。通过仔细处理坐标系约定和利用准确的传感器数据，我们可以有效地将无人机捕捉到的图像信息与现实世界的地理空间联系起来，为各种高级应用提供基础支持。