193 lines
5.8 KiB
Markdown
193 lines
5.8 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
|
|
|
|
```
|
|
Search Algorithm → UAV Controller → MAVROS → ArduPilot
|
|
→ UGV Controller → cmd_vel
|
|
```
|
|
|
|
| Node | Function |
|
|
|------|----------|
|
|
| `uav_controller` | GUIDED mode control, auto-arm |
|
|
| `ugv_controller` | Differential drive control |
|
|
| `search` | Spiral, lawnmower, Lévy walk algorithms |
|
|
|
|
#### 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/
|
|
├── main.py # Entry point
|
|
├── vision/
|
|
│ ├── object_detector.py # ArUco marker detection
|
|
│ └── camera_processor.py # Gazebo camera feeds
|
|
├── navigation/
|
|
│ ├── search.py # Search algorithm controller
|
|
│ ├── flight_tracker.py # Live flight visualization
|
|
│ └── patterns/
|
|
│ ├── spiral.py # Expanding square spiral
|
|
│ ├── lawnmower.py # Back-and-forth lanes
|
|
│ └── levy.py # Lévy walk random search
|
|
├── control/
|
|
│ ├── uav_controller.py # UAV flight control (pymavlink)
|
|
│ └── ugv_controller.py # UGV drive control (gz.transport)
|
|
├── safety/
|
|
│ └── geofence.py # GPS boundary enforcement
|
|
└── utils/
|
|
├── config.py # YAML config loader
|
|
├── convert.py # Unit/coordinate conversions
|
|
├── helpers.py # Distance utilities
|
|
└── recorder.py # Video recording & cloud upload
|
|
```
|
|
|
|
## 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
|
|
```
|
|
|
|
### Speed Limits
|
|
|
|
```yaml
|
|
WPNAV_SPEED: 150 # Horizontal speed (cm/s)
|
|
WPNAV_ACCEL: 100 # Horizontal acceleration (cm/s/s)
|
|
WPNAV_SPEED_UP: 100 # Climb speed (cm/s)
|
|
WPNAV_SPEED_DN: 75 # Descent speed (cm/s)
|
|
WPNAV_ACCEL_Z: 75 # Vertical acceleration (cm/s/s)
|
|
LOIT_SPEED: 150 # Loiter speed (cm/s)
|
|
```
|