02-03-08 无人机自主导航

1. 核心概念

1.1 自主导航概述

自主导航是指无人机在无人工干预情况下，根据传感器数据和预设任务，自主完成路径规划、避障、定位的能力。

核心能力：

• 感知（Perception）：环境感知、障碍物检测
• 规划（Planning）：路径规划、轨迹生成
• 控制（Control）：位置控制、姿态控制
• 决策（Decision）：任务管理、异常处理

技术架构：

传感器层：
├── GPS/RTK（全局定位）
├── IMU（姿态测量）
├── 激光雷达（障碍检测）
├── 视觉相机（环境感知）
└── 超声波/毫米波雷达（近距探测）

算法层：
├── 定位算法（EKF/粒子滤波）
├── 建图算法（SLAM）
├── 路径规划（A*/RRT）
├── 轨迹生成（最小snap）
└── 避障算法（APF/DWA）

控制层：
├── 位置控制器
├── 速度控制器
├── 姿态控制器
└── 电机分配

1.2 坐标系统

常用坐标系：

地球坐标系（ECEF）：
- 原点：地球质心
- 用途：GPS定位

大地坐标系（LLA）：
- 纬度、经度、高度
- 用途：全局导航

东北天坐标系（ENU）：
- 原点：起飞点
- X轴：东，Y轴：北，Z轴：天
- 用途：局部导航

机体坐标系（Body Frame）：
- 原点：无人机重心
- X轴：前，Y轴：右，Z轴：下
- 用途：姿态控制

坐标转换：

def lla_to_enu(lat, lon, alt, lat0, lon0, alt0):
    """LLA转ENU坐标

    Args:
        lat, lon, alt: 目标点经纬度高度
        lat0, lon0, alt0: 原点经纬度高度

    Returns:
        east, north, up: ENU坐标（米）
    """
    # WGS84参数
    a = 6378137.0  # 长半轴
    b = 6356752.314245  # 短半轴
    e2 = 1 - (b**2 / a**2)

    # 转弧度
    lat_rad = np.radians(lat)
    lon_rad = np.radians(lon)
    lat0_rad = np.radians(lat0)
    lon0_rad = np.radians(lon0)

    # 计算N
    N = a / np.sqrt(1 - e2 * np.sin(lat_rad)**2)
    N0 = a / np.sqrt(1 - e2 * np.sin(lat0_rad)**2)

    # ECEF坐标
    x = (N + alt) * np.cos(lat_rad) * np.cos(lon_rad)
    y = (N + alt) * np.cos(lat_rad) * np.sin(lon_rad)
    z = (N * (1 - e2) + alt) * np.sin(lat_rad)

    x0 = (N0 + alt0) * np.cos(lat0_rad) * np.cos(lon0_rad)
    y0 = (N0 + alt0) * np.cos(lat0_rad) * np.sin(lon0_rad)
    z0 = (N0 * (1 - e2) + alt0) * np.sin(lat0_rad)

    # ECEF差值
    dx = x - x0
    dy = y - y0
    dz = z - z0

    # 旋转矩阵
    sin_lat = np.sin(lat0_rad)
    cos_lat = np.cos(lat0_rad)
    sin_lon = np.sin(lon0_rad)
    cos_lon = np.cos(lon0_rad)

    east = -sin_lon * dx + cos_lon * dy
    north = -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz
    up = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz

    return east, north, up

# 使用示例
home_lat, home_lon, home_alt = 31.230416, 121.473701, 10.0  # 上海
target_lat, target_lon, target_alt = 31.231416, 121.474701, 50.0

e, n, u = lla_to_enu(target_lat, target_lon, target_alt,
                     home_lat, home_lon, home_alt)
print(f"ENU: East={e:.2f}m, North={n:.2f}m, Up={u:.2f}m")

1.3 导航模式

常见模式：

手动模式（Manual）：
- 完全手动控制
- 用于调试和测试

定高模式（Altitude Hold）：
- 自动保持高度
- 手动控制水平运动

定点模式（Position Hold）：
- 自动保持位置和高度
- GPS辅助悬停

航点模式（Waypoint）：
- 按预设航点飞行
- 自动任务执行

返航模式（RTH）：
- 自动返回起飞点
- 低电量/失控触发

跟随模式（Follow Me）：
- 跟随目标移动
- 视觉/GPS跟踪

2. 路径规划算法

2.1 A*算法

原理：

启发式搜索：f(n) = g(n) + h(n)

其中：
- g(n)：从起点到节点n的实际代价
- h(n)：从节点n到终点的估计代价（启发函数）
- f(n)：节点n的总代价估计

启发函数选择：
- 曼哈顿距离：h = |x1-x2| + |y1-y2|
- 欧几里得距离：h = √((x1-x2)² + (y1-y2)²)
- 对角距离：h = max(|x1-x2|, |y1-y2|)

实现（Python）：

from typing import List, Tuple

class AStarPlanner:
    """A*路径规划器"""

    def __init__(self, grid_size=1.0):
        self.grid_size = grid_size

    def plan(self, start: Tuple[float, float],
             goal: Tuple[float, float],
             obstacles: List[Tuple[float, float]],
             map_bounds: Tuple[float, float, float, float]) -> List[Tuple[float, float]]:
        """A*路径规划

        Args:
            start: 起点(x, y)
            goal: 终点(x, y)
            obstacles: 障碍物列表
            map_bounds: 地图边界(min_x, max_x, min_y, max_y)

        Returns:
            path: 路径点列表
        """
        # 栅格化
        min_x, max_x, min_y, max_y = map_bounds
        grid_map = self._create_grid_map(obstacles, map_bounds)

        # 转换为栅格坐标
        start_grid = self._world_to_grid(start[0], start[1], min_x, min_y)
        goal_grid = self._world_to_grid(goal[0], goal[1], min_x, min_y)

        # A*搜索
        open_set = []
        heapq.heappush(open_set, (0, start_grid))

        came_from = {}
        g_score = {start_grid: 0}
        f_score = {start_grid: self._heuristic(start_grid, goal_grid)}

        while open_set:
            current = heapq.heappop(open_set)[1]

            if current == goal_grid:
                # 重建路径
                path = self._reconstruct_path(came_from, current)
                # 转换回世界坐标
                return [(self._grid_to_world(p[0], p[1], min_x, min_y))
                        for p in path]

            # 8邻域搜索
            for dx, dy in [(-1,0), (1,0), (0,-1), (0,1),
                           (-1,-1), (-1,1), (1,-1), (1,1)]:
                neighbor = (current[0] + dx, current[1] + dy)

                # 边界检查
                if not self._is_valid(neighbor, grid_map):
                    continue

                # 对角线代价为√2
                move_cost = np.sqrt(dx**2 + dy**2)
                tentative_g = g_score[current] + move_cost

                if neighbor not in g_score or tentative_g < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g
                    f = tentative_g + self._heuristic(neighbor, goal_grid)
                    f_score[neighbor] = f
                    heapq.heappush(open_set, (f, neighbor))

        return []  # 无路径

    def _heuristic(self, a: Tuple[int, int], b: Tuple[int, int]) -> float:
        """启发函数（欧几里得距离）"""
        return np.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)

    def _create_grid_map(self, obstacles, bounds):
        """创建栅格地图"""
        min_x, max_x, min_y, max_y = bounds
        width = int((max_x - min_x) / self.grid_size)
        height = int((max_y - min_y) / self.grid_size)

        grid_map = np.zeros((height, width), dtype=bool)

        # 标记障碍物
        for ox, oy in obstacles:
            gx, gy = self._world_to_grid(ox, oy, min_x, min_y)
            if 0 <= gx < width and 0 <= gy < height:
                grid_map[gy, gx] = True

        return grid_map

    def _world_to_grid(self, x, y, min_x, min_y):
        """世界坐标转栅格坐标"""
        gx = int((x - min_x) / self.grid_size)
        gy = int((y - min_y) / self.grid_size)
        return (gx, gy)

    def _grid_to_world(self, gx, gy, min_x, min_y):
        """栅格坐标转世界坐标"""
        x = gx * self.grid_size + min_x
        y = gy * self.grid_size + min_y
        return (x, y)

    def _is_valid(self, pos, grid_map):
        """检查位置是否有效"""
        gx, gy = pos
        height, width = grid_map.shape

        if gx < 0 or gx >= width or gy < 0 or gy >= height:
            return False

        return not grid_map[gy, gx]

    def _reconstruct_path(self, came_from, current):
        """重建路径"""
        path = [current]
        while current in came_from:
            current = came_from[current]
            path.append(current)
        path.reverse()
        return path

# 使用示例
planner = AStarPlanner(grid_size=1.0)

start = (0, 0)
goal = (50, 50)
obstacles = [(10, 10), (10, 11), (11, 10), (20, 20), (20, 21)]
bounds = (0, 60, 0, 60)

path = planner.plan(start, goal, obstacles, bounds)
print(f"路径点数: {len(path)}")
for i, (x, y) in enumerate(path):
    print(f"  P{i}: ({x:.1f}, {y:.1f})")

2.2 RRT算法

快速随机树（RRT）原理：

1. 从起点开始构建树
2. 随机采样新点
3. 找到树中最近节点
4. 向新点方向扩展固定步长
5. 检查碰撞，无碰撞则加入树
6. 重复直到到达目标

实现：

class RRTPlanner:
    """RRT路径规划器"""

    def __init__(self, step_size=5.0, max_iter=1000, goal_threshold=2.0):
        self.step_size = step_size
        self.max_iter = max_iter
        self.goal_threshold = goal_threshold

    def plan(self, start, goal, obstacles, bounds):
        """RRT路径规划"""
        # 初始化树
        tree = {0: {'pos': start, 'parent': None}}
        node_id = 1

        for _ in range(self.max_iter):
            # 随机采样（10%概率直接采样目标）
            if np.random.rand() < 0.1:
                rand_point = goal
            else:
                rand_point = self._random_sample(bounds)

            # 找最近节点
            nearest_id = self._nearest_node(tree, rand_point)
            nearest_pos = tree[nearest_id]['pos']

            # 扩展
            new_pos = self._steer(nearest_pos, rand_point)

            # 碰撞检测
            if self._is_collision_free(nearest_pos, new_pos, obstacles):
                tree[node_id] = {'pos': new_pos, 'parent': nearest_id}

                # 检查是否到达目标
                if self._distance(new_pos, goal) < self.goal_threshold:
                    # 添加目标节点
                    tree[node_id + 1] = {'pos': goal, 'parent': node_id}
                    # 重建路径
                    return self._extract_path(tree, node_id + 1)

                node_id += 1

        return []  # 未找到路径

    def _random_sample(self, bounds):
        """随机采样"""
        min_x, max_x, min_y, max_y = bounds
        x = np.random.uniform(min_x, max_x)
        y = np.random.uniform(min_y, max_y)
        return (x, y)

    def _nearest_node(self, tree, point):
        """找最近节点"""
        min_dist = float('inf')
        nearest_id = 0

        for node_id, node in tree.items():
            dist = self._distance(node['pos'], point)
            if dist < min_dist:
                min_dist = dist
                nearest_id = node_id

        return nearest_id

    def _steer(self, from_pos, to_pos):
        """从from_pos向to_pos扩展step_size"""
        dist = self._distance(from_pos, to_pos)

        if dist < self.step_size:
            return to_pos

        ratio = self.step_size / dist
        x = from_pos[0] + (to_pos[0] - from_pos[0]) * ratio
        y = from_pos[1] + (to_pos[1] - from_pos[1]) * ratio

        return (x, y)

    def _distance(self, p1, p2):
        """欧几里得距离"""
        return np.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)

    def _is_collision_free(self, from_pos, to_pos, obstacles):
        """碰撞检测（简化版）"""
        # 线段采样检测
        steps = int(self._distance(from_pos, to_pos) / 0.5) + 1

        for i in range(steps):
            t = i / steps
            x = from_pos[0] + (to_pos[0] - from_pos[0]) * t
            y = from_pos[1] + (to_pos[1] - from_pos[1]) * t

            # 检查是否与障碍物碰撞
            for ox, oy in obstacles:
                if self._distance((x, y), (ox, oy)) < 2.0:
                    return False

        return True

    def _extract_path(self, tree, goal_id):
        """提取路径"""
        path = []
        current_id = goal_id

        while current_id is not None:
            path.append(tree[current_id]['pos'])
            current_id = tree[current_id]['parent']

        path.reverse()
        return path

# 使用示例
rrt = RRTPlanner(step_size=5.0, max_iter=2000)
path = rrt.plan(start, goal, obstacles, bounds)
print(f"RRT路径点数: {len(path)}")

2.3 Dijkstra算法

最短路径算法：

def dijkstra(graph, start, goal):
    """Dijkstra最短路径

    Args:
        graph: 邻接表 {node: [(neighbor, cost), ...]}
        start: 起点
        goal: 终点

    Returns:
        path: 最短路径
        cost: 总代价
    """

    # 初始化
    distances = {node: float('inf') for node in graph}
    distances[start] = 0
    came_from = {}

    pq = [(0, start)]

    while pq:
        current_dist, current = heapq.heappop(pq)

        if current == goal:
            # 重建路径
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            path.reverse()
            return path, distances[goal]

        if current_dist > distances[current]:
            continue

        for neighbor, cost in graph[current]:
            distance = current_dist + cost

            if distance < distances[neighbor]:
                distances[neighbor] = distance
                came_from[neighbor] = current
                heapq.heappush(pq, (distance, neighbor))

    return [], float('inf')  # 无路径

3. 轨迹生成

3.1 最小Snap轨迹

原理：

优化目标：最小化jerk的积分（平滑轨迹）

代价函数：
J = ∫(d⁴x/dt⁴)² dt

约束：
- 起点/终点位置、速度、加速度
- 中间航点位置
- 最大速度/加速度限制

实现：

from scipy.optimize import minimize

class MinimumSnapTrajectory:
    """最小Snap轨迹生成"""

    def __init__(self, waypoints, T):
        """
        Args:
            waypoints: 航点列表 [(x, y, z), ...]
            T: 每段时间
        """
        self.waypoints = np.array(waypoints)
        self.T = T
        self.n_seg = len(waypoints) - 1

    def generate(self):
        """生成轨迹"""
        # 为每个轴生成轨迹
        traj_x = self._generate_1d(self.waypoints[:, 0])
        traj_y = self._generate_1d(self.waypoints[:, 1])
        traj_z = self._generate_1d(self.waypoints[:, 2])

        return traj_x, traj_y, traj_z

    def _generate_1d(self, waypoints):
        """生成单轴轨迹（7阶多项式）"""
        # 每段轨迹用7阶多项式表示
        # p(t) = a0 + a1*t + a2*t² + ... + a7*t⁷

        n_coef = 8  # 8个系数
        n_var = self.n_seg * n_coef

        # 构建约束矩阵
        A_eq = []
        b_eq = []

        # 航点位置约束
        for i in range(self.n_seg + 1):
            if i == 0:
                # 起点
                A_row = np.zeros(n_var)
                A_row[0] = 1  # p(0) = a0
                A_eq.append(A_row)
                b_eq.append(waypoints[i])
            elif i == self.n_seg:
                # 终点
                A_row = np.zeros(n_var)
                idx = (i - 1) * n_coef
                t = self.T
                for j in range(n_coef):
                    A_row[idx + j] = t**j
                A_eq.append(A_row)
                b_eq.append(waypoints[i])
            else:
                # 中间点连续性
                # 段i-1的终点 = 段i的起点
                A_row = np.zeros(n_var)
                idx1 = (i - 1) * n_coef
                idx2 = i * n_coef
                t = self.T
                for j in range(n_coef):
                    A_row[idx1 + j] = t**j
                    A_row[idx2 + j] = -1 if j == 0 else 0
                A_eq.append(A_row)
                b_eq.append(0)

        # 起点/终点速度、加速度约束（设为0）
        # ... 省略详细约束构建 ...

        A_eq = np.array(A_eq)
        b_eq = np.array(b_eq)

        # 二次规划求解
        # min 0.5 * x^T * H * x
        H = self._build_cost_matrix(n_var)

        result = minimize(
            lambda x: 0.5 * x.T @ H @ x,
            x0=np.zeros(n_var),
            constraints={'type': 'eq', 'fun': lambda x: A_eq @ x - b_eq}
        )

        return result.x.reshape(self.n_seg, n_coef)

    def _build_cost_matrix(self, n_var):
        """构建代价矩阵（snap项）"""
        H = np.zeros((n_var, n_var))

        for seg in range(self.n_seg):
            idx = seg * 8
            # snap = d⁴p/dt⁴ = 24*a4 + 120*a5*t + 360*a6*t² + 840*a7*t³
            # ∫snap² dt = ...
            # 简化实现
            H[idx:idx+8, idx:idx+8] += np.eye(8) * 0.01

        return H

    def evaluate(self, coeffs, t):
        """评估轨迹点"""
        seg = int(t / self.T)
        if seg >= self.n_seg:
            seg = self.n_seg - 1

        t_local = t - seg * self.T
        pos = sum(coeffs[seg, i] * t_local**i for i in range(8))

        return pos

# 使用示例
waypoints = np.array([
    [0, 0, 0],
    [10, 10, 5],
    [20, 5, 10],
    [30, 0, 5]
])

traj_gen = MinimumSnapTrajectory(waypoints, T=2.0)
traj_x, traj_y, traj_z = traj_gen.generate()

# 评估轨迹
t_samples = np.linspace(0, 6.0, 100)
path = []
for t in t_samples:
    x = traj_gen.evaluate(traj_x, t)
    y = traj_gen.evaluate(traj_y, t)
    z = traj_gen.evaluate(traj_z, t)
    path.append((x, y, z))

3.2 贝塞尔曲线

三次贝塞尔曲线：

def bezier_curve(P0, P1, P2, P3, num_points=100):
    """三次贝塞尔曲线

    Args:
        P0, P1, P2, P3: 控制点
        num_points: 采样点数

    Returns:
        curve: 曲线点列表
    """
    t = np.linspace(0, 1, num_points)

    curve = []
    for ti in t:
        # B(t) = (1-t)³*P0 + 3(1-t)²t*P1 + 3(1-t)t²*P2 + t³*P3
        B = (1 - ti)**3 * P0 + \
            3 * (1 - ti)**2 * ti * P1 + \
            3 * (1 - ti) * ti**2 * P2 + \
            ti**3 * P3
        curve.append(B)

    return np.array(curve)

# 使用示例
P0 = np.array([0, 0])
P1 = np.array([5, 10])
P2 = np.array([15, 10])
P3 = np.array([20, 0])

curve = bezier_curve(P0, P1, P2, P3)

4. 自主起降

4.1 自动起飞

起飞流程：

1. 预检（Pre-flight Check）
   - 传感器校准
   - GPS定位检查
   - 电池电量检查
   - 电机测试

2. 解锁（Arming）
   - 发送解锁命令
   - 等待确认

3. 起飞（Takeoff）
   - 缓慢增加油门
   - 达到目标高度
   - 切换到定点模式

代码实现：

from pymavlink import mavutil

class AutoTakeoff:
    """自动起飞"""

    def __init__(self, connection_string='udp:0.0.0.0:14550'):
        self.master = mavutil.mavlink_connection(connection_string)
        self.master.wait_heartbeat()

    def pre_flight_check(self):
        """飞行前检查"""
        print("执行飞行前检查...")

        # 检查GPS
        msg = self.master.recv_match(type='GPS_RAW_INT', blocking=True, timeout=5)
        if not msg or msg.fix_type < 3:
            print("❌ GPS未定位")
            return False
        print("✓ GPS已定位")

        # 检查电池
        msg = self.master.recv_match(type='BATTERY_STATUS', blocking=True, timeout=5)
        if msg and msg.battery_remaining < 30:
            print("❌ 电量不足")
            return False
        print("✓ 电池正常")

        # 检查IMU
        msg = self.master.recv_match(type='ATTITUDE', blocking=True, timeout=5)
        if not msg:
            print("❌ IMU无数据")
            return False
        print("✓ IMU正常")

        return True

    def arm(self):
        """解锁电机"""
        print("解锁电机...")

        self.master.mav.command_long_send(
            self.master.target_system,
            self.master.target_component,
            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
            0,
            1,  # arm
            0, 0, 0, 0, 0, 0
        )

        # 等待ACK
        ack = self.master.recv_match(type='COMMAND_ACK', blocking=True, timeout=5)
        if ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED:
            print("✓ 解锁成功")
            return True
        else:
            print("❌ 解锁失败")
            return False

    def takeoff(self, altitude=10.0):
        """起飞到指定高度

        Args:
            altitude: 目标高度（米）
        """
        print(f"起飞到{altitude}米...")

        # 发送起飞命令
        self.master.mav.command_long_send(
            self.master.target_system,
            self.master.target_component,
            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
            0,
            0, 0, 0, 0, 0, 0,
            altitude
        )

        # 监控高度
        start_time = time.time()
        while True:
            msg = self.master.recv_match(type='GLOBAL_POSITION_INT',
                                        blocking=True, timeout=1)
            if msg:
                current_alt = msg.relative_alt / 1000.0
                print(f"  当前高度: {current_alt:.1f}m")

                if current_alt >= altitude * 0.95:
                    print("✓ 到达目标高度")
                    break

            if time.time() - start_time > 60:
                print("❌ 起飞超时")
                return False

            time.sleep(0.5)

        return True

    def execute(self, altitude=10.0):
        """执行自动起飞"""
        # 飞行前检查
        if not self.pre_flight_check():
            return False

        # 解锁
        if not self.arm():
            return False

        time.sleep(2)

        # 起飞
        if not self.takeoff(altitude):
            return False

        print("✓ 自动起飞完成")
        return True

# 使用示例
takeoff = AutoTakeoff()
if takeoff.execute(altitude=10.0):
    print("起飞成功，进入任务...")

4.2 精准降落

视觉降落：

class VisionLanding:
    """视觉精准降落"""

    def __init__(self, marker_size=0.3):
        """
        Args:
            marker_size: 降落标记尺寸（米）
        """
        self.marker_size = marker_size
        self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50)
        self.aruco_params = cv2.aruco.DetectorParameters_create()

    def detect_marker(self, image, camera_matrix, dist_coeffs):
        """检测ArUco标记

        Returns:
            position: 标记位置(x, y, z)，相机坐标系
            detected: 是否检测到
        """
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

        # 检测标记
        corners, ids, _ = cv2.aruco.detectMarkers(
            gray, self.aruco_dict, parameters=self.aruco_params
        )

        if ids is None or len(ids) == 0:
            return None, False

        # 估计姿态
        rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
            corners, self.marker_size, camera_matrix, dist_coeffs
        )

        # 返回位置（相机坐标系）
        position = tvecs[0][0]
        return position, True

    def calculate_control(self, marker_pos, target_pos=(0, 0, -2.0)):
        """计算控制量

        Args:
            marker_pos: 标记位置(x, y, z)
            target_pos: 目标位置（相机坐标系）

        Returns:
            vx, vy, vz: 速度命令（m/s）
        """
        # PID控制
        Kp = 0.5

        error_x = target_pos[0] - marker_pos[0]
        error_y = target_pos[1] - marker_pos[1]
        error_z = target_pos[2] - marker_pos[2]

        vx = Kp * error_x
        vy = Kp * error_y
        vz = Kp * error_z

        # 限速
        max_speed = 1.0
        vx = np.clip(vx, -max_speed, max_speed)
        vy = np.clip(vy, -max_speed, max_speed)
        vz = np.clip(vz, -max_speed, max_speed)

        return vx, vy, vz

# 使用示例（配合MAVLink）
def landing_loop(mav_connection, vision_landing, camera):
    """降落循环"""
    # 相机内参（需标定）
    camera_matrix = np.array([
        [500, 0, 320],
        [0, 500, 240],
        [0, 0, 1]
    ])
    dist_coeffs = np.zeros(5)

    while True:
        # 获取图像
        ret, frame = camera.read()
        if not ret:
            continue

        # 检测标记
        marker_pos, detected = vision_landing.detect_marker(
            frame, camera_matrix, dist_coeffs
        )

        if detected:
            print(f"标记位置: {marker_pos}")

            # 计算控制
            vx, vy, vz = vision_landing.calculate_control(marker_pos)

            # 发送速度命令
            mav_connection.master.mav.set_position_target_local_ned_send(
                0,
                mav_connection.master.target_system,
                mav_connection.master.target_component,
                mavutil.mavlink.MAV_FRAME_BODY_NED,
                0b0000111111000111,  # 只控制速度
                0, 0, 0,
                vx, vy, vz,
                0, 0, 0,
                0, 0
            )

            # 检查是否着陆
            if abs(marker_pos[2]) < 0.3:
                print("着陆")
                break
        else:
            print("未检测到标记")

        time.sleep(0.1)

5. 避障集成

5.1 人工势场法（APF）

原理：

吸引力：F_att = k_att * (goal - current)
排斥力：F_rep = k_rep * (1/d - 1/d0) * (1/d²) * n

其中：
- d：到障碍物距离
- d0：影响范围
- n：障碍物到机器人的单位向量

实现：

class ArtificialPotentialField:
    """人工势场避障"""

    def __init__(self, k_att=1.0, k_rep=100.0, d0=5.0):
        self.k_att = k_att  # 吸引力增益
        self.k_rep = k_rep  # 排斥力增益
        self.d0 = d0        # 障碍物影响范围

    def compute_force(self, current, goal, obstacles):
        """计算合力

        Args:
            current: 当前位置(x, y)
            goal: 目标位置(x, y)
            obstacles: 障碍物列表[(x, y), ...]

        Returns:
            force: 合力(fx, fy)
        """
        current = np.array(current)
        goal = np.array(goal)

        # 吸引力
        F_att = self.k_att * (goal - current)

        # 排斥力
        F_rep = np.zeros(2)
        for obs in obstacles:
            obs = np.array(obs)
            diff = current - obs
            distance = np.linalg.norm(diff)

            if distance < self.d0:
                direction = diff / distance
                magnitude = self.k_rep * (1.0/distance - 1.0/self.d0) / (distance**2)
                F_rep += magnitude * direction

        # 合力
        F_total = F_att + F_rep

        return F_total

# 使用示例
apf = ArtificialPotentialField()

current = (0, 0)
goal = (50, 50)
obstacles = [(25, 25), (30, 30)]

force = apf.compute_force(current, goal, obstacles)
print(f"合力: {force}")

# 速度命令
max_speed = 5.0
speed = np.linalg.norm(force)
if speed > max_speed:
    force = force / speed * max_speed

vx, vy = force

5.2 动态窗口法（DWA）

原理：

1. 采样速度空间（v, ω）
2. 预测轨迹
3. 评估轨迹（目标方向、障碍物距离、速度）
4. 选择最优轨迹

实现：

class DWA:
    """动态窗口法避障"""

    def __init__(self):
        self.max_v = 2.0      # 最大线速度
        self.max_w = 1.0      # 最大角速度
        self.max_accel = 1.0  # 最大加速度
        self.dt = 0.1         # 时间步长
        self.predict_time = 2.0  # 预测时间

    def compute_velocity(self, state, goal, obstacles):
        """计算速度命令

        Args:
            state: 当前状态(x, y, theta, v, w)
            goal: 目标(x, y)
            obstacles: 障碍物[(x, y), ...]

        Returns:
            v, w: 速度命令
        """
        x, y, theta, v, w = state

        # 动态窗口
        v_range = [max(0, v - self.max_accel * self.dt),
                   min(self.max_v, v + self.max_accel * self.dt)]
        w_range = [max(-self.max_w, w - self.max_accel * self.dt),
                   min(self.max_w, w + self.max_accel * self.dt)]

        # 采样
        best_v, best_w = v, w
        max_score = -float('inf')

        for vi in np.linspace(v_range[0], v_range[1], 10):
            for wi in np.linspace(w_range[0], w_range[1], 10):
                # 预测轨迹
                traj = self._predict_trajectory(state, vi, wi)

                # 评估
                score = self._evaluate(traj, goal, obstacles)

                if score > max_score:
                    max_score = score
                    best_v, best_w = vi, wi

        return best_v, best_w

    def _predict_trajectory(self, state, v, w):
        """预测轨迹"""
        x, y, theta, _, _ = state
        traj = []

        for _ in np.arange(0, self.predict_time, self.dt):
            x += v * np.cos(theta) * self.dt
            y += v * np.sin(theta) * self.dt
            theta += w * self.dt
            traj.append((x, y))

        return traj

    def _evaluate(self, traj, goal, obstacles):
        """评估轨迹"""
        # 目标方向得分
        end_point = traj[-1]
        goal_score = -np.sqrt((end_point[0] - goal[0])**2 +
                             (end_point[1] - goal[1])**2)

        # 障碍物得分
        obs_score = float('inf')
        for point in traj:
            for obs in obstacles:
                dist = np.sqrt((point[0] - obs[0])**2 +
                              (point[1] - obs[1])**2)
                obs_score = min(obs_score, dist)

        # 速度得分（倾向于高速）
        # ...

        total_score = goal_score + obs_score * 2.0
        return total_score

6. 行业应用

案例1：亚马逊Prime Air

配置：

• 导航：GPS + 视觉SLAM
• 避障：多目立体视觉 + 激光雷达
• 降落：视觉精准降落（ArUco标记）
• 航程：24km

技术特点：

• 混合翼设计（固定翼+多旋翼）
• 全自主飞行
• 视距外（BVLOS）运营

案例2：DJI Mavic 3

导航系统：

• APAS 5.0（高级避障系统）
• 6向避障（前后左右上下）
• 返航路径规划（避障）

传感器配置：

• 双目视觉×4
• 红外传感器×2
• ToF传感器×1

7. 测试与验证

7.1 仿真测试

Gazebo仿真：

# 启动PX4 SITL
cd PX4-Autopilot
make px4_sitl gazebo

# 启动MAVROS
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

# 运行导航节点
rosrun my_nav autonomous_nav_node

Python仿真：

class DroneSimulator:
    """无人机仿真器"""

    def __init__(self):
        self.position = np.array([0.0, 0.0, 0.0])
        self.velocity = np.array([0.0, 0.0, 0.0])
        self.dt = 0.01

    def update(self, acceleration):
        """更新状态"""
        self.velocity += acceleration * self.dt
        self.position += self.velocity * self.dt

    def step(self, control_input):
        """仿真步进"""
        # 控制输入转加速度
        acceleration = control_input  # 简化

        self.update(acceleration)
        return self.position.copy()

7.2 实机测试

测试流程：

1. 地面测试
   - 电机响应测试
   - 传感器校准
   - 通信链路测试

2. 悬停测试
   - 定点悬停稳定性
   - GPS精度测试
   - 风扰抗性

3. 航点飞行测试
   - 简单矩形航线
   - 复杂航线跟随
   - 航点精度评估

4. 避障测试
   - 静态障碍物
   - 动态障碍物
   - 应急避障

5. 返航测试
   - 正常返航
   - 低电量返航
   - 失控保护

8. 故障处理

8.1 常见问题

现象	原因	解决方法
位置漂移	GPS精度差	切换RTK/视觉定位
轨迹偏离	风扰过大	增大PID增益
避障失效	传感器盲区	多传感器融合
返航失败	航点丢失	记录起飞点
碰撞	算法延迟	优化算法/硬件

8.2 应急处理

失控保护：

def emergency_handler(state, timeout=5.0):
    """应急处理"""
    if state['signal_lost_time'] > timeout:
        # 失控超时
        if state['altitude'] > 50:
            # 高空：返航
            return 'RTH'
        else:
            # 低空：原地降落
            return 'LAND'

    if state['battery'] < 10:
        # 低电量：强制降落
        return 'FORCE_LAND'

    if state['obstacle_distance'] < 1.0:
        # 紧急避障
        return 'EMERGENCY_STOP'

    return 'NORMAL'