178 lines
5.1 KiB
Markdown
178 lines
5.1 KiB
Markdown
# System Architecture
|
|
|
|
## Overview
|
|
|
|
```
|
|
┌─────────────────────────────────────────────────────────────────┐
|
|
│ Simulation │
|
|
├───────────────────┬─────────────────────┬───────────────────────┤
|
|
│ Gazebo Harmonic │ ArduPilot SITL │ ROS 2 Nodes │
|
|
│ (Physics/3D) │ (Flight Control) │ (Perception/Nav) │
|
|
└───────────────────┴─────────────────────┴───────────────────────┘
|
|
│ │ │
|
|
└────────────────────┼──────────────────────┘
|
|
│
|
|
┌─────────┴─────────┐
|
|
│ MAVLink/MAVROS │
|
|
└───────────────────┘
|
|
```
|
|
|
|
## Components
|
|
|
|
### 1. Gazebo Harmonic
|
|
|
|
**Role:** 3D simulation, physics, sensors
|
|
|
|
- **World simulation**: Ground plane, lighting, physics
|
|
- **UAV model**: Iris quadcopter with cameras
|
|
- **UGV model**: Differential drive rover
|
|
- **Sensors**: Cameras, IMU, rangefinder
|
|
|
|
### 2. ArduPilot SITL
|
|
|
|
**Role:** Flight controller simulation
|
|
|
|
- **EKF3**: State estimation using external vision
|
|
- **Flight modes**: GUIDED, LOITER, RTL, LAND
|
|
- **Motor mixing**: Quadcopter dynamics
|
|
- **Failsafe**: Battery, geofence, communication
|
|
|
|
**Key Parameters (GPS-Denied):**
|
|
```
|
|
GPS_TYPE 0 # GPS disabled
|
|
EK3_SRC1_POSXY 6 # External nav for position
|
|
EK3_SRC1_VELXY 6 # External nav for velocity
|
|
VISO_TYPE 1 # MAVLink vision input
|
|
ARMING_CHECK 0 # Disabled for simulation
|
|
```
|
|
|
|
### 3. ROS 2 Nodes
|
|
|
|
#### Vision Pipeline
|
|
|
|
```
|
|
Camera → Visual Odometry → Position Estimator → Controller
|
|
↓
|
|
Optical Flow ─────────────┘
|
|
```
|
|
|
|
| Node | Function |
|
|
|------|----------|
|
|
| `visual_odom_node` | ORB feature tracking, pose estimation |
|
|
| `optical_flow_node` | Lucas-Kanade velocity estimation |
|
|
| `position_estimator` | Weighted average sensor fusion |
|
|
| `ekf_fusion_node` | Extended Kalman Filter fusion |
|
|
|
|
#### Control Pipeline
|
|
|
|
```
|
|
Mission Planner → UAV Controller → MAVROS → ArduPilot
|
|
→ UGV Controller → cmd_vel
|
|
```
|
|
|
|
| Node | Function |
|
|
|------|----------|
|
|
| `uav_controller` | GUIDED mode control, auto-arm |
|
|
| `ugv_controller` | Differential drive control |
|
|
| `mission_planner` | Multi-vehicle coordination |
|
|
|
|
#### Safety Pipeline
|
|
|
|
```
|
|
GPS → Geofence Monitor → Failsafe Handler → Emergency Action
|
|
```
|
|
|
|
| Node | Function |
|
|
|------|----------|
|
|
| `geofence_node` | GPS-based boundary monitoring |
|
|
| `failsafe_handler` | Vision loss, battery, emergency |
|
|
|
|
## Data Flow
|
|
|
|
### Position Estimation
|
|
|
|
```
|
|
1. Camera captures frames (30 Hz)
|
|
2. ORB detects features
|
|
3. Essential matrix computed
|
|
4. Relative motion estimated
|
|
5. Position integrated
|
|
6. Published to /uav/visual_odometry/pose
|
|
```
|
|
|
|
### Control Loop
|
|
|
|
```
|
|
1. Target received (/uav/setpoint_position)
|
|
2. Current position from VO or MAVROS
|
|
3. Error computed
|
|
4. Velocity command generated
|
|
5. Sent via MAVROS to ArduPilot
|
|
6. ArduPilot executes motor commands
|
|
```
|
|
|
|
### Geofencing (GPS Only)
|
|
|
|
```
|
|
1. GPS fix received
|
|
2. Check against polygon boundary
|
|
3. Check altitude limits
|
|
4. If breach: trigger RTL/LAND
|
|
5. Navigation continues using VO (not GPS)
|
|
```
|
|
|
|
## Coordinate Frames
|
|
|
|
| Frame | Description |
|
|
|-------|-------------|
|
|
| `odom` | Local origin (takeoff point) |
|
|
| `base_link` | Vehicle body frame |
|
|
| `map` | World frame (aligned with odom) |
|
|
|
|
**NED Convention:**
|
|
- X = North (forward)
|
|
- Y = East (right)
|
|
- Z = Down (negative altitude)
|
|
|
|
## File Structure
|
|
|
|
```
|
|
src/
|
|
├── vision/
|
|
│ ├── visual_odometry.py # Feature tracking VO
|
|
│ ├── optical_flow.py # LK optical flow
|
|
│ └── camera_processor.py # Image processing
|
|
├── localization/
|
|
│ ├── position_estimator.py # Weighted fusion
|
|
│ └── ekf_fusion.py # EKF fusion
|
|
├── navigation/
|
|
│ ├── local_planner.py # Path planning
|
|
│ └── waypoint_follower.py # Waypoint tracking
|
|
├── control/
|
|
│ ├── uav_controller.py # UAV flight control
|
|
│ ├── ugv_controller.py # UGV drive control
|
|
│ └── mission_planner.py # Coordination
|
|
└── safety/
|
|
├── geofence_monitor.py # GPS boundaries
|
|
└── failsafe_handler.py # Emergency handling
|
|
```
|
|
|
|
## Configuration
|
|
|
|
### ArduPilot EKF Sources
|
|
|
|
```yaml
|
|
EK3_SRC1_POSXY: 6 # External Nav
|
|
EK3_SRC1_POSZ: 1 # Barometer
|
|
EK3_SRC1_VELXY: 6 # External Nav
|
|
EK3_SRC1_YAW: 6 # External Nav
|
|
```
|
|
|
|
### Sensor Weights
|
|
|
|
```yaml
|
|
vo_weight: 0.6 # Visual odometry
|
|
optical_flow: 0.3 # Optical flow
|
|
imu: 0.1 # IMU integration
|
|
```
|