基于视场角，高度重叠率，选装角度的无人机区域巡检航线规划

最新推荐文章于 2024-09-13 11:21:01 发布

1028_左傲杰

最新推荐文章于 2024-09-13 11:21:01 发布

阅读量111

点赞数 1

文章标签：无人机

本文链接：https://blog.csdn.net/weixin_53221438/article/details/141676547

版权

import math
from shapely.geometry import Polygon, LineString




def calculate_route_with_angle(area_coords, fov_angle, altitude, overlap, line_angle):
    def line_intersection(p1, p2, p3, p4):
        """计算两条线段的交点"""
        line1 = LineString([p1, p2])
        line2 = LineString([p3, p4])
        intersection = line1.intersection(line2)
        if intersection.is_empty:
            return None
        elif intersection.geom_type == 'Point':
            return (intersection.x, intersection.y)
        else:
            return None  # 如果交点不在多边形内

    # 1. 创建多边形
    coords = [(coord['longitude'], coord['latitude']) for coord in area_coords]
    polygon = Polygon(coords)

    # 2. 计算视场角对应的宽度和间隔
    fov_width = 2 * altitude * math.tan(math.radians(fov_angle) / 2)
    angle_rad = math.radians(line_angle)
    effective_width = fov_width / math.cos(angle_rad)
    interval_lat = effective_width * (1 - overlap) / 111320  # 纬度间隔 (1度纬度大约对应111.32公里)

    # 3. 确定经纬度范围
    min_lat, max_lat = polygon.bounds[1], polygon.bounds[3]
    min_lon, max_lon = polygon.bounds[0], polygon.bounds[2]
    # 4. 生成航点
    route_points = []
    add_start_first = True  # 控制添加起点和终点的顺序
    line_length = max_lon - min_lon


    current_lat = max_lat
    stop_lat_up=max_lat
    stop_lat_down=min_lat-math.tan(angle_rad) * line_length

    if line_angle<0:
        current_lat=min_lat
        interval_lat=-interval_lat
        stop_lat_up=max_lat-math.tan(angle_rad) * line_length
        stop_lat_down=min_lat





    while current_lat >= stop_lat_down and current_lat<=stop_lat_up:
        # 生成线

        x1 = min_lon
        y1 = current_lat
        x2 = max_lon
        y2 = math.tan(angle_rad) * line_length + y1
        print(current_lat)
        line = LineString([(x1,y1), (x2,y2)])

        # 计算线与多边形边界的交点
        intersections = []
        for i in range(len(polygon.exterior.coords) - 1):
            edge = LineString([polygon.exterior.coords[i], polygon.exterior.coords[i + 1]])
            intersection = line_intersection(line.coords[0], line.coords[1], edge.coords[0], edge.coords[1])
            if intersection:
                intersections.append(intersection)

        # 根据交点添加航点
        if intersections:
            intersections.sort(key=lambda p: p[0])  # 按经度排序
            if add_start_first:
                route_points.extend([{'longitude': x, 'latitude': y} for x, y in intersections])
            else:
                route_points.extend([{'longitude': x, 'latitude': y} for x, y in reversed(intersections)])

        # 移动到下一个水平线
        current_lat -= interval_lat
        add_start_first = not add_start_first  # 交换添加顺序

    return route_points


# 测试数据
area_coords = [
    {'longitude': 118.78726800840519, 'latitude': 31.896095735979607},
    {'longitude': 118.78808916838733, 'latitude': 31.896215386879994},
    {'longitude': 118.78827926108342, 'latitude': 31.895232837223894},
    {'longitude': 118.78823833643426, 'latitude': 31.895121895339866},
    {'longitude': 118.78748136837368, 'latitude': 31.89501134575386}
]

fov_angle = 45  # 视场角，单位为度
altitude = 25  # 飞行高度，单位为米
overlap = 0.6  # 重叠率
line_angle = 50  # 线的角度，相对于水平线

# 调用函数
route = calculate_route_with_angle(area_coords, fov_angle, altitude, overlap, line_angle)

print(route)