目录

背景

算法思路

代码实现

验证

背景

在城市交通巡检中如何进行车辆违停判断很重要，一个方法是通过精确坐标判断车辆中心是否位于违停框中，我们假设无人机坐标已知，并且无人机云台镜头垂直地面朝下，可根据图像分辨率、无人机参数、无人机坐标、识别目标位置等信息，解算出识别目标的具体坐标。

无人机采用大疆机场 2 自带的 Matrice3TD 无人机，参数如下：

水平视场角：84 °

图像分辨率：800 X 600

无人机自带 RTK ，实时测定无人机当前坐标信息，包括经度、纬度、相对高度以及航向角

算法思路

通过水平视场角和高度计算出影像实际长度，再由实际长度和影像分辨率计算单个栅格长度，再由yolo 目标识别矩形框计算中心栅格坐标，并计算与无人机地面投影点的栅格坐标差，从而计算实际矢量坐标差，再由航向角计算出目标中心最终坐标。

简要解算：

注意，本算法仅考虑无人机镜头垂直朝下的情况！！！

代码实现

参数定义：

# 参数
Horizontal_view = 84  # 无人机水平视场角
Resolution_X = 800   # X 轴栅格数
Resolution_Y = 600   # Y 轴栅格数
R = 6371000  # 地球半径（米）

获取单个像素长度：

def calculate_pixel_length(h):"""计算像素长度:param h: 无人机相对高度（米）:return: 像素长度（米）"""# 计算水平视场角的一半half_horizontal_view = Horizontal_view / 2.0# 图像长度l = 2.0 * h * math.tan(math.radians(half_horizontal_view))distance = l / Resolution_Xreturn distance

可利用水平视场角、分辨率和高度 h 计算

解算目标坐标：

def calculate_target_coordinates(x1, y1, x2, y2, heading_angle, longitude, latitude, h):"""计算目标坐标，仅考虑目标位于北半球、东半球:param x1: 目标左上角 X 轴像素位置:param y1: 目标左上角 Y 轴像素位置:param x2: 目标右下角 X 轴像素位置:param y2: 目标右下角 Y 轴像素位置:param heading_angle: 无人机航向角（度）:param longitude: 无人机经度（度）:param latitude: 无人机纬度（度）:param h: 无人机相对高度（米）:return: 目标坐标 (x, y)"""pixel_length = calculate_pixel_length(h)# 目标中心点像素坐标target_x = (x1 + x2) / 2.0target_y = (y1 + y2) / 2.0# 无人机像素中心坐标drone_x = Resolution_X / 2.0drone_y = Resolution_Y / 2.0# 计算相对于中心点的像素位移，yolo 识别输出图像的坐标轴向下为 y 轴增加，需反转，不然后面坐标系无法转换raster_dx = target_x - drone_xraster_dy = drone_y - target_y# 实际距离vector_dx = raster_dx * pixel_lengthvector_dy = raster_dy * pixel_length# 图像坐标系转为地理坐标系，计算地理坐标系的分量，可理解为将图像坐标系逆时针旋转到与地理坐标系重合dx = vector_dx * math.cos(math.radians(heading_angle)) - vector_dy * math.sin(math.radians(heading_angle))dy = vector_dx * math.sin(math.radians(heading_angle)) + vector_dy * math.cos(math.radians(heading_angle))# 解算坐标，一纬度约为111km，一经度则还需乘cos(纬度)target_lon = longitude + (180 / math.pi) * (dx / (R * math.cos(latitude)))target_lat = latitude + (180 / math.pi) * (dy / R)# 返回目标坐标return target_lon, target_lat

计算出目标相对于中心点的位移后，需要将 y 轴反转，因为图像 y 轴正方向是朝下的，与地理坐标系相反，然后再将图像坐标系作坐标系的旋转，旋转到地理坐标系上，从而能计算出目标点位移对于地理坐标系的分量，从而解算目标点坐标。

验证

if __name__ == '__main__':x, y = calculate_target_coordinates(10, 20, 20, 30, 20, 115.89, 28.68, 5)print(f"Target Coordinates: Longitude={x}, Latitude={y}")

输出结果：