基于无人机 RTK 和 yolov8 的目标定位算法

目录

背景

算法思路

代码实现

验证

背景

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

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

水平视场角：84 °

图像分辨率：800 X 600

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

算法思路

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

简要解算：

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

代码实现

参数定义：

python 复制代码

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

获取单个像素长度：

python 复制代码

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_X
    return distance

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

解算目标坐标：

python 复制代码

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.0
    target_y = (y1 + y2) / 2.0

    # 无人机像素中心坐标
    drone_x = Resolution_X / 2.0
    drone_y = Resolution_Y / 2.0

    # 计算相对于中心点的像素位移，yolo 识别输出图像的坐标轴向下为 y 轴增加，需反转，不然后面坐标系无法转换
    raster_dx = target_x - drone_x
    raster_dy = drone_y - target_y

    # 实际距离
    vector_dx = raster_dx * pixel_length
    vector_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轴正方向是朝下的，与地理坐标系相反，然后再将图像坐标系作坐标系的旋转，旋转到地理坐标系上，从而能计算出目标点位移对于地理坐标系的分量，从而解算目标点坐标。

验证

python 复制代码

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}")

输出结果：

大家可以自行验证下，理论上是没有问题的。

至于调用，在 yolo 识别后，会输出 bbox 等参数，然后对每个帧循环目标再调用此算法即可。

感谢观看！！！