Scripts Update
This commit is contained in:
@@ -1,126 +1,120 @@
|
||||
# Architecture Overview
|
||||
# Architecture
|
||||
|
||||
## System Architecture
|
||||
System architecture for GPS-denied UAV/UGV navigation.
|
||||
|
||||
## Overview
|
||||
|
||||
The system uses vision-based navigation with GPS reserved only for geofencing.
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────┐
|
||||
│ SIMULATION LAYER │
|
||||
│ ┌──────────────────┐ ┌──────────────────┐ ┌───────────────┐ │
|
||||
│ │ Gazebo World │ │ ArduPilot SITL │ │ MAVROS │ │
|
||||
│ │ (Physics, Viz) │ │ (Autopilot) │ │ (ROS Bridge) │ │
|
||||
│ └────────┬─────────┘ └────────┬─────────┘ └───────┬───────┘ │
|
||||
└───────────┼─────────────────────┼─────────────────────┼─────────┘
|
||||
│ │ │
|
||||
▼ ▼ ▼
|
||||
┌─────────────────────────────────────────────────────────────────┐
|
||||
│ SENSOR LAYER │
|
||||
│ ┌────────────┐ ┌────────────┐ ┌────────────┐ ┌──────────┐ │
|
||||
│ │ Forward │ │ Downward │ │ IMU │ │ Range- │ │
|
||||
│ │ Camera │ │ Camera │ │ │ │ finder │ │
|
||||
│ └─────┬──────┘ └─────┬──────┘ └─────┬──────┘ └────┬─────┘ │
|
||||
└────────┼───────────────┼───────────────┼──────────────┼────────┘
|
||||
│ │ │ │
|
||||
▼ ▼ ▼ ▼
|
||||
┌─────────────────────────────────────────────────────────────────┐
|
||||
│ PERCEPTION LAYER │
|
||||
│ ┌─────────────────────┐ ┌────────────────┐ ┌──────────────┐ │
|
||||
│ │ Visual Odometry │ │ Optical Flow │ │ Landmark │ │
|
||||
│ │ (ORB/SIFT/SURF) │ │ (Lucas-Kanade) │ │ Detection │ │
|
||||
│ └──────────┬──────────┘ └───────┬────────┘ └──────┬───────┘ │
|
||||
└─────────────┼─────────────────────┼──────────────────┼──────────┘
|
||||
│ │ │
|
||||
▼ ▼ ▼
|
||||
┌─────────────────────────────────────────────────────────────────┐
|
||||
│ LOCALIZATION LAYER │
|
||||
│ ┌──────────────────────┐ │
|
||||
│ │ Position Estimator │ │
|
||||
│ │ (EKF Sensor Fusion)│ │
|
||||
│ └──────────┬───────────┘ │
|
||||
└───────────────────────────────┼─────────────────────────────────┘
|
||||
│
|
||||
▼
|
||||
┌─────────────────────────────────────────────────────────────────┐
|
||||
│ NAVIGATION LAYER │
|
||||
│ ┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐ │
|
||||
│ │ Local Planner │ │ Obstacle │ │ Waypoint │ │
|
||||
│ │ │ │ Avoidance │ │ Follower │ │
|
||||
│ └────────┬────────┘ └────────┬────────┘ └────────┬────────┘ │
|
||||
└───────────┼────────────────────┼────────────────────┼───────────┘
|
||||
│ │ │
|
||||
▼ ▼ ▼
|
||||
┌─────────────────────────────────────────────────────────────────┐
|
||||
│ CONTROL LAYER │
|
||||
│ ┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐ │
|
||||
│ │ UAV Controller │ │ UGV Controller │ │ Mission Planner │ │
|
||||
│ │ │ │ │ │ │ │
|
||||
│ └────────┬────────┘ └────────┬────────┘ └────────┬────────┘ │
|
||||
└───────────┼────────────────────┼────────────────────┼───────────┘
|
||||
│ │ │
|
||||
▼ ▼ ▼
|
||||
┌─────────────────────────────────────────────────────────────────┐
|
||||
│ SAFETY LAYER │
|
||||
│ ┌─────────────────────────┐ ┌─────────────────────────────┐ │
|
||||
│ │ Geofence Monitor │ │ Failsafe Handler │ │
|
||||
│ │ (GPS-based only) │ │ (Vision loss, battery) │ │
|
||||
│ └─────────────────────────┘ └─────────────────────────────┘ │
|
||||
└─────────────────────────────────────────────────────────────────┘
|
||||
Vision Sensors (Cameras)
|
||||
|
|
||||
v
|
||||
Visual Odometry & Optical Flow
|
||||
|
|
||||
v
|
||||
Position Estimator (EKF)
|
||||
|
|
||||
v
|
||||
ArduPilot Flight Controller
|
||||
|
|
||||
v
|
||||
Motor Control
|
||||
```
|
||||
|
||||
## Module Descriptions
|
||||
## Components
|
||||
|
||||
### Vision Module (`src/vision/`)
|
||||
- **camera_processor.py**: Image preprocessing, undistortion
|
||||
- **visual_odometry.py**: Feature-based pose estimation
|
||||
- **optical_flow.py**: Velocity from image flow
|
||||
- **object_detector.py**: Landmark/obstacle detection
|
||||
- **visual_servoing.py**: Vision-based control
|
||||
### Perception
|
||||
|
||||
### Localization Module (`src/localization/`)
|
||||
- **position_estimator.py**: Sensor fusion for position
|
||||
- **ekf_fusion.py**: Extended Kalman Filter implementation
|
||||
- **landmark_tracker.py**: Track known visual features
|
||||
- **Forward Camera**: Visual odometry, feature tracking
|
||||
- **Downward Camera**: Optical flow, ground plane detection
|
||||
- **IMU**: Angular velocity, acceleration
|
||||
|
||||
### Navigation Module (`src/navigation/`)
|
||||
- **local_planner.py**: Path planning in local frame
|
||||
- **obstacle_avoidance.py**: Reactive obstacle avoidance
|
||||
- **waypoint_follower.py**: Sequential waypoint navigation
|
||||
### Localization
|
||||
|
||||
### Control Module (`src/control/`)
|
||||
- **uav_controller.py**: Quadcopter flight control
|
||||
- **ugv_controller.py**: Ground vehicle control
|
||||
- **mission_planner.py**: Multi-vehicle coordination
|
||||
- **Visual Odometry**: Estimates motion from camera images
|
||||
- **Optical Flow**: Velocity estimation from downward camera
|
||||
- **EKF Fusion**: Combines all sensor inputs into position estimate
|
||||
|
||||
### Safety Module (`src/safety/`)
|
||||
- **geofence_monitor.py**: GPS-based boundary checking
|
||||
- **failsafe_handler.py**: Emergency procedures
|
||||
### Navigation
|
||||
|
||||
- **Waypoint Navigation**: Relative coordinates (meters from origin)
|
||||
- **Path Planning**: Collision-free paths using local map
|
||||
- **Position Hold**: Maintain position using vision
|
||||
|
||||
### Control
|
||||
|
||||
- **ArduPilot**: Flight controller firmware
|
||||
- **MAVROS**: ROS interface to ArduPilot
|
||||
- **Velocity/Position Control**: Low-level motor commands
|
||||
|
||||
### Safety
|
||||
|
||||
- **Geofencing**: GPS-based boundary checking (safety only)
|
||||
- **Altitude Limits**: Maximum flight ceiling
|
||||
- **Failsafe**: RTL on signal loss or boundary breach
|
||||
|
||||
## Coordinate Frames
|
||||
|
||||
| Frame | Description |
|
||||
|-------|-------------|
|
||||
| body | Attached to vehicle, X forward, Y right, Z down |
|
||||
| odom | Origin at takeoff, accumulates drift |
|
||||
| map | Fixed world frame (may be corrected) |
|
||||
|
||||
All navigation commands use local coordinates relative to takeoff point.
|
||||
|
||||
## Data Flow
|
||||
|
||||
```
|
||||
Camera Image → Feature Detection → Feature Matching →
|
||||
Essential Matrix → Relative Pose → EKF Update →
|
||||
Local Position → Navigation Controller →
|
||||
Velocity Commands → MAVROS → ArduPilot
|
||||
Camera Images
|
||||
|
|
||||
v
|
||||
Feature Detection (ORB/SIFT)
|
||||
|
|
||||
v
|
||||
Feature Matching (frame to frame)
|
||||
|
|
||||
v
|
||||
Motion Estimation (Essential Matrix)
|
||||
|
|
||||
v
|
||||
EKF Update (fuse with IMU)
|
||||
|
|
||||
v
|
||||
Position Estimate (x, y, z, roll, pitch, yaw)
|
||||
|
|
||||
v
|
||||
ArduPilot (external position input)
|
||||
|
|
||||
v
|
||||
PID Control -> Motor PWM
|
||||
```
|
||||
|
||||
## ArduPilot Integration
|
||||
|
||||
ArduPilot receives external position via MAVLink:
|
||||
|
||||
1. Visual odometry → `VISION_POSITION_ESTIMATE`
|
||||
2. EKF uses external source (EK3_SRC1_POSXY=6)
|
||||
3. GPS disabled for navigation (GPS_TYPE=0 or EK3_SRC1_POSXY!=1)
|
||||
|
||||
## Geofencing
|
||||
|
||||
GPS is only used for safety boundaries:
|
||||
|
||||
1. GPS position → lat/lon
|
||||
2. Check against polygon/circle boundary
|
||||
3. If outside: trigger RTL or LAND
|
||||
|
||||
Navigation continues using vision regardless of GPS status.
|
||||
|
||||
## ROS 2 Topics
|
||||
|
||||
### Sensor Topics
|
||||
- `/uav/camera/forward/image_raw`
|
||||
- `/uav/camera/downward/image_raw`
|
||||
- `/uav/imu/data`
|
||||
- `/uav/rangefinder/range`
|
||||
|
||||
### Perception Topics
|
||||
- `/uav/visual_odometry/pose`
|
||||
- `/uav/optical_flow/velocity`
|
||||
|
||||
### Control Topics
|
||||
- `/uav/mavros/setpoint_position/local`
|
||||
- `/uav/mavros/setpoint_velocity/cmd_vel`
|
||||
- `/ugv/cmd_vel`
|
||||
|
||||
### Safety Topics
|
||||
- `/geofence/status`
|
||||
- `/failsafe/active`
|
||||
|
||||
| Topic | Type | Description |
|
||||
|-------|------|-------------|
|
||||
| /uav/camera/forward/image_raw | sensor_msgs/Image | Forward camera |
|
||||
| /uav/camera/downward/image_raw | sensor_msgs/Image | Downward camera |
|
||||
| /mavros/local_position/pose | geometry_msgs/PoseStamped | Current position |
|
||||
| /mavros/setpoint_position/local | geometry_msgs/PoseStamped | Target position |
|
||||
| /mavros/imu/data | sensor_msgs/Imu | IMU data |
|
||||
|
||||
Reference in New Issue
Block a user