好的，我们将编写一个 Python 函数来计算无人车在给定路径上的里程和行驶时长。我们将结合提供的文件内容和图片信息来完成这个任务。 ### 题目重述 根据给定的路径（一系列栅格坐标和车头朝向），计算无人车在该路径上的总里程和行驶时长。 ### 代码实现 ```python import numpy as np def calculate_distance_and_time(path, cell_size=5, k=5, vehicle_type='A'): """ 根据路径计算无人车的总里程和行驶时长 :param path: 路径列表，每个元素为 (x, y, heading) 表示栅格坐标和车头朝向（单位为度） :param cell_size: 栅格边长，默认为5米 :param k: 高程变化因子，默认为5 :param vehicle_type: 车型，默认为 'A' 型无人车 :return: 总里程（单位为米）和行驶时长（单位为秒） """ # 定义各车型的速度和耗电系数 performance_data = { 'A': { 'speeds': [(0, 10, 30), (10, 20, 20), (20, 30, 10)], 'coefficients': [(0, 10, 1.0), (10, 20, 1.5), (20, 30, 2.0)] } # 其他车型可以在这里添加 } # 获取指定车型的数据 data = performance_data.get(vehicle_type) if not data: raise ValueError(f"未知车型：{vehicle_type}") def get_speed_and_coefficient(slope_deg): """根据坡度获取速度和耗电系数""" speeds = data['speeds'] coefficients = data['coefficients'] speed = None coefficient = None for (min_slope, max_slope, speed_value) in speeds: if min_slope <= slope_deg < max_slope: speed = speed_value break for (min_slope, max_slope, coeff_value) in coefficients: if min_slope <= slope_deg < max_slope: coefficient = coeff_value break if speed is None or coefficient is None: raise ValueError(f"坡度 {slope_deg}° 不在任何已知区间内") return speed, coefficient def calculate_slope_aspect(elevations): """根据3x3邻域的高程计算坡度和坡向""" dz_dx = k * (elevations[(1, 0)] + 2 * elevations[(1, 1)] + elevations[(1, -1)] - elevations[(-1, 0)] - 2 * elevations[(-1, -1)] - elevations[(-1, 1)]) / (8 * cell_size) dz_dy = k * (elevations[(-1, -1)] + 2 * elevations[(0, -1)] + elevations[(1, -1)] - elevations[(-1, 1)] - 2 * elevations[(0, 1)] - elevations[(1, 1)]) / (8 * cell_size) slope_rad = np.arctan(np.sqrt(dz_dx**2 + dz_dy**2)) slope_deg = slope_rad * 180 / np.pi if dz_dx == 0 and dz_dy == 0: aspect_deg = 0 elif dz_dx < 0: aspect_deg = 270 + np.degrees(np.arctan2(dz_dy, dz_dx)) elif dz_dx > 0: aspect_deg = np.degrees(np.arctan2(dz_dy, dz_dx)) elif dz_dx == 0 and dz_dy > 0: aspect_deg = 90 elif dz_dx == 0 and dz_dy < 0: aspect_deg = 270 else: # dz_dx == 0 and dz_dy == 0 (水平面) aspect_deg = 0 aspect_deg = (aspect_deg + 360) % 360 return slope_deg, aspect_deg def get_3x3_elevations(map_data, x, y): """获取3x3邻域的高程""" elevations = {} for dx in range(-1, 2): for dy in range(-1, 2): nx, ny = x + dx, y + dy if 0 <= nx < map_data.shape[1] and 0 <= ny < map_data.shape[0]: elevations[(dx, dy)] = map_data[ny, nx] else: elevations[(dx, dy)] = None return elevations total_distance = 0.0 total_time = 0.0 # 假设有一个地图文件用于获取高程数据 with rasterio.open("map.tif") as src: map_data = src.read(1) for i in range(1, len(path)): x_i, y_i, heading_i = path[i] x_i_minus_1, y_i_minus_1, heading_i_minus_1 = path[i - 1] # 获取3x3邻域的高程 elevations = get_3x3_elevations(map_data, x_i, y_i) if any(v is None for v in elevations.values()): raise ValueError("路径中的某些栅格位于地图边界外，无法计算高程") # 计算坡度和坡向 slope_deg, _ = calculate_slope_aspect(elevations) # 获取速度和耗电系数 speed_kmh, _ = get_speed_and_coefficient(slope_deg) speed_ms = speed_kmh * 1000 / 3600 # 转换为米/秒 # 计算里程加权系数 w(ΔLi, Δθi) delta_x = abs(x_i - x_i_minus_1) delta_y = abs(y_i - y_i_minus_1) delta_heading = abs(heading_i - heading_i_minus_1) if delta_x + delta_y == 1: if delta_heading == 0: w = 1 elif delta_heading == 45: w = 1.5 elif delta_heading == 90: w = 2 elif delta_x + delta_y == 2: if delta_heading == 0: w = np.sqrt(2) elif delta_heading == 45: w = np.sqrt(2) + 0.5 elif delta_heading == 90: w = np.sqrt(2) + 1 else: raise ValueError("无效的路径移动：ΔLi 必须为1或2") # 计算当前栅格的里程 Di distance = w * cell_size total_distance += distance # 计算当前栅格的行驶时间 Ti time_seconds = distance / speed_ms if speed_ms != 0 else float('inf') total_time += time_seconds return total_distance, total_time # 示例调用 path = [ (100, 100, 0), (101, 100, 0), (101, 101, 90), (102, 101, 90) ] total_distance, total_time = calculate_distance_and_time(path) print(f"总里程为：{total_distance:.2f} 米，行驶时长为：{total_time:.2f} 秒") ``` ### 代码解析 1. **定义性能数据**：在 `performance_data` 字典中定义了各车型的速度和耗电系数，以元组列表的形式表示每个坡度区间的速度和耗电系数。 2. **获取指定车型的数据**：根据传入的 `vehicle_type` 参数，从 `performance_data` 中获取对应车型的数据。如果车型不存在，抛出异常。 3. **计算坡度和坡向**：根据3×3邻域的高程计算坡度和坡向，确保在计算路径中的每个栅格时都能获取正确的高程数据。 4. **计算里程加权系数**：根据 `ΔLi` 和 `Δθi` 查找表2中的加权系数 `w(ΔLi, Δθi)`，并计算当前栅格的里程 `Di`。 5. **计算速度和耗电系数**：根据坡度查找相应速度和耗电系数。 6. **计算总里程和行驶时长**：遍历路径中的每个栅格，累加总里程和行驶时长，最终返回结果。 ### 知识点 1. **路径遍历**：通过遍历路径中的每个栅格，计算每个栅格的里程和行驶时间。 2. **区间匹配**：通过遍历区间列表，找到与输入坡度匹配的速度和耗电系数。 3. **里程加权系数**：根据表2中的规则计算里程加权系数，并用于计算当前栅格的里程。 如果你有任何进一步的问题或需要更多的解释，请随时告知！