无人机 A* 路径规划系统

This guide explains a compact A* path-planning workflow for a drone software stack. It combines a grid map, an A* search routine, MAVLink position commands, and landing/disarm confirmation so the mission flow is easier to reason about and safer to test.

System Overview

The planner treats the operating area as a grid. Free cells are marked as 0, blocked cells as 1. A* evaluates candidate nodes using path cost plus a Euclidean-distance heuristic, then returns an ordered path from start to goal.

The MAVLink layer then converts each grid waypoint into local NED position targets. For a lab or course workflow, this is a useful bridge between algorithm design and flight-control integration.

Control Flow

1. Connect: Open a MAVLink connection and wait for heartbeat.
2. Prepare state: Request extended state updates and switch to GUIDED mode.
3. Start mission: Arm motors and command takeoff to a fixed altitude.
4. Follow path: Send each planned grid point as a local position target.
5. Recover safely: Land, wait for landed state, then confirm disarm.

Reference Implementation

The script below is kept as a single readable reference so it can be copied into a lab environment and adapted for real mission constraints.

#!/usr/bin/env python3

import time
import math
import heapq
import numpy as np
from pymavlink import mavutil

# =========================
# A* CORE
# =========================

class Node:
 def __init__(self, x, y):
 self.x = x
 self.y = y
 self.g = float("inf")
 self.h = 0
 self.f = float("inf")
 self.parent = None

 def __lt__(self, other):
 return self.f < other.f

 def __eq__(self, other):
 return self.x == other.x and self.y == other.y

 def __hash__(self):
 return hash((self.x, self.y))


class AStar:
 def __init__(self, grid, start, goal):
 self.grid = grid
 self.rows, self.cols = grid.shape
 self.start = Node(*start)
 self.goal = Node(*goal)
 self.nodes = {}

 def get_node(self, x, y):
 if (x, y) not in self.nodes:
 self.nodes[(x, y)] = Node(x, y)
 return self.nodes[(x, y)]

 def heuristic(self, n):
 return math.hypot(n.x - self.goal.x, n.y - self.goal.y)

 def neighbors(self, n):
 for dx, dy in [(-1,0),(1,0),(0,-1),(0,1)]:
 x, y = n.x + dx, n.y + dy
 if 0 <= x < self.rows and 0 <= y < self.cols:
 if self.grid[x][y] == 0:
 yield self.get_node(x, y)

 def search(self):
 self.start.g = 0
 self.start.h = self.heuristic(self.start)
 self.start.f = self.start.h

 open_set = []
 heapq.heappush(open_set, self.start)
 closed = set()

 while open_set:
 current = heapq.heappop(open_set)

 if current == self.goal:
 return self.reconstruct(current)

 closed.add(current)

 for nb in self.neighbors(current):
 if nb in closed:
 continue

 g_new = current.g + 1.0
 if g_new < nb.g:
 nb.parent = current
 nb.g = g_new
 nb.h = self.heuristic(nb)
 nb.f = nb.g + nb.h
 heapq.heappush(open_set, nb)

 return None

 def reconstruct(self, n):
 path = []
 while n:
 path.append((n.x, n.y))
 n = n.parent
 return path[::-1]

# =========================
# MAVLINK
# =========================

def connect_sitl():
 print("Trying to connect")
 master = mavutil.mavlink_connection("udp:127.0.0.1:14551")
 master.wait_heartbeat()
 print(" Connected to SITL")
 return master

def request_extended_state(master):
 master.mav.command_long_send(
 master.target_system,
 master.target_component,
 mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
 0,
 mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE,
 1_000_000,
 0, 0, 0, 0, 0
 )
 print(" Requested EXTENDED_SYS_STATE stream")

def set_guided_mode(master, timeout=5):
 print(" Setting GUIDED mode...")
 master.mav.set_mode_send(
 master.target_system,
 mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
 4
 )

 start = time.time()
 while time.time() - start < timeout:
 hb = master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
 if hb and hb.custom_mode == 4:
 print(" GUIDED mode confirmed")
 return True

 print(" GUIDED mode not confirmed")
 return False

def land_drone(master):
 print(" Landing...")
 master.mav.set_mode_send(
 master.target_system,
 mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
 9
 )

def wait_until_landed(master, timeout=30):
 print("⏳ Waiting for landed state...")
 start = time.time()

 while time.time() - start < timeout:
 msg = master.recv_match(type='EXTENDED_SYS_STATE', blocking=True, timeout=1)
 if msg and msg.landed_state == mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND:
 print(" Landed (confirmed)")
 return True

 print(" Landing timeout")
 return False

def wait_until_disarmed(master, timeout=30):
 print("⏳ Waiting for disarm...")
 start = time.time()

 while time.time() - start < timeout:
 hb = master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
 if hb is None:
 continue

 armed = hb.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
 if not armed:
 print(" Motors disarmed — landing complete")
 return True

 print(" Disarm timeout")
 return False

def arm_and_takeoff(master, alt=5):
 if not set_guided_mode(master):
 return False

 print(" Arming motors...")
 master.arducopter_arm()
 master.motors_armed_wait()
 print(" Motors armed")

 print(f" Taking off to {alt} m")
 master.mav.command_long_send(
 master.target_system,
 master.target_component,
 mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
 0,
 0, 0, 0, 0, 0, 0,
 alt
 )

 time.sleep(5)
 return True

def send_position(master, x, y, z):
 master.mav.set_position_target_local_ned_send(
 0,
 master.target_system,
 master.target_component,
 mavutil.mavlink.MAV_FRAME_LOCAL_NED,
 0b0000111111111000,
 x, y, z,
 0,0,0,
 0,0,0,
 0,0
 )

def follow_path(master, path, hold_time=2.0, rate=5):
 print(" Following path...")
 dt = 1.0 / rate

 for gx, gy in path:
 print(f"→ Going to ({gx}, {gy})")
 start = time.time()
 while time.time() - start < hold_time:
 send_position(master, gx, gy, -5)
 time.sleep(dt)

# =========================
# MAIN
# =========================

def main():
 grid = np.zeros((10,10))
 grid[2:8,3] = 1
 grid[5,1:9] = 1

 astar = AStar(grid, (0,0), (9,9))
 path = astar.search()

 if not path:
 print(" No path")
 return

 master = connect_sitl()
 request_extended_state(master)

 arm_and_takeoff(master, alt=5)
 follow_path(master, path)

 time.sleep(2)
 land_drone(master)
 wait_until_landed(master)
 wait_until_disarmed(master)

 print(" Mission complete")

if __name__ == "__main__":
 main()

Technical Notes

What To Improve Next

• Add altitude, velocity, and geofence constraints to the waypoint executor.
• Convert grid cells into a calibrated local coordinate frame.
• Connect obstacle updates from SLAM or visual perception.
• Add mission abort behavior for stale telemetry, lost heartbeat, or low battery.