212 lines
4.3 KiB
Markdown
212 lines
4.3 KiB
Markdown
# GPS-Denied Navigation
|
||
|
||
## Overview
|
||
|
||
This system enables UAV/UGV navigation **without GPS** by using:
|
||
|
||
1. **Visual Odometry** - Camera-based pose estimation
|
||
2. **Optical Flow** - Velocity estimation from downward camera
|
||
3. **IMU Integration** - Short-term dead reckoning
|
||
4. **EKF Fusion** - Combine all sensors
|
||
|
||
**GPS is ONLY used for geofencing (safety boundaries).**
|
||
|
||
## How It Works
|
||
|
||
### Visual Odometry
|
||
|
||
```
|
||
Frame N-1 Frame N
|
||
│ │
|
||
▼ ▼
|
||
┌────────┐ ┌────────┐
|
||
│Features│──│Features│ → Match features
|
||
└────────┘ └────────┘
|
||
│ │
|
||
└─────┬─────┘
|
||
▼
|
||
Essential Matrix → Rotation + Translation
|
||
```
|
||
|
||
**Algorithm:**
|
||
1. Detect ORB/SIFT features in current frame
|
||
2. Match with previous frame features
|
||
3. Compute Essential Matrix (RANSAC)
|
||
4. Recover rotation and translation
|
||
5. Integrate to get absolute pose
|
||
|
||
### Optical Flow
|
||
|
||
```
|
||
Downward Camera
|
||
│
|
||
▼
|
||
┌───────┐
|
||
│ Image │ → Lucas-Kanade flow
|
||
└───────┘
|
||
│
|
||
▼
|
||
Pixel velocity × Altitude / Focal length = Ground velocity
|
||
```
|
||
|
||
**Works best at:**
|
||
- Altitudes 0.5m - 10m
|
||
- Textured ground surfaces
|
||
- Stable lighting
|
||
|
||
### Sensor Fusion
|
||
|
||
```
|
||
Visual Odometry ─┬─→ Weighted Average ─→ Position Estimate
|
||
Optical Flow ────┤
|
||
IMU ─────────────┘
|
||
```
|
||
|
||
**Weights (configurable):**
|
||
- Visual Odometry: 60%
|
||
- Optical Flow: 30%
|
||
- IMU: 10%
|
||
|
||
## ArduPilot Configuration
|
||
|
||
### EKF3 External Navigation
|
||
|
||
```
|
||
# GPS Type - Disabled
|
||
GPS_TYPE 0
|
||
GPS_TYPE2 0
|
||
|
||
# EKF3 Source Configuration
|
||
AHRS_EKF_TYPE 3 # Use EKF3
|
||
EK3_ENABLE 1
|
||
EK2_ENABLE 0
|
||
|
||
# Position from External Nav
|
||
EK3_SRC1_POSXY 6 # External Nav
|
||
EK3_SRC1_POSZ 1 # Barometer
|
||
EK3_SRC1_VELXY 6 # External Nav
|
||
EK3_SRC1_VELZ 0 # None
|
||
EK3_SRC1_YAW 6 # External Nav
|
||
|
||
# Vision Position Input
|
||
VISO_TYPE 1 # MAVLink
|
||
VISO_POS_X 0.1 # Camera offset
|
||
VISO_DELAY_MS 50 # Processing delay
|
||
```
|
||
|
||
### Arming Checks
|
||
|
||
```
|
||
# For simulation, disable all
|
||
ARMING_CHECK 0
|
||
|
||
# For real flight, keep safety checks
|
||
# ARMING_CHECK 14 # Skip GPS only
|
||
```
|
||
|
||
## Coordinate Frames
|
||
|
||
### Local NED Frame
|
||
|
||
```
|
||
North (X+)
|
||
↑
|
||
│
|
||
│
|
||
West ←─────┼─────→ East (Y+)
|
||
│
|
||
│
|
||
↓
|
||
(Down is Z+)
|
||
```
|
||
|
||
**All navigation uses LOCAL coordinates:**
|
||
- Takeoff point is origin (0, 0, 0)
|
||
- No global GPS coordinates
|
||
- Relative waypoints only
|
||
|
||
### Example Waypoints
|
||
|
||
```python
|
||
# Square pattern (5m sides)
|
||
waypoints = [
|
||
(5, 0, -5), # North 5m, altitude 5m
|
||
(5, 5, -5), # North-East corner
|
||
(0, 5, -5), # East
|
||
(0, 0, -5), # Back to start
|
||
]
|
||
```
|
||
|
||
## Limitations
|
||
|
||
### Visual Odometry
|
||
- **Scale drift**: Position error grows over time
|
||
- **Texture needed**: Poor in featureless environments
|
||
- **Lighting**: Affected by shadows, brightness changes
|
||
- **Motion blur**: High-speed motion degrades accuracy
|
||
|
||
### Optical Flow
|
||
- **Altitude dependent**: Accuracy varies with height
|
||
- **Ground texture**: Needs visible ground features
|
||
- **Tilt sensitivity**: Assumes mostly horizontal flight
|
||
|
||
### Mitigation Strategies
|
||
|
||
1. **Loop closure**: Return to known positions
|
||
2. **Landmark detection**: ArUco markers for correction
|
||
3. **Multi-sensor fusion**: Combine VO + OF + IMU
|
||
4. **Flight patterns**: Minimize cumulative drift
|
||
|
||
## Geofencing (GPS Only)
|
||
|
||
GPS is used **only** for safety:
|
||
|
||
```python
|
||
# Geofence uses GPS coordinates
|
||
fence_points = [
|
||
(47.397742, 8.545594), # Corner 1 (lat, lon)
|
||
(47.398242, 8.545594), # Corner 2
|
||
(47.398242, 8.546094), # Corner 3
|
||
(47.397742, 8.546094), # Corner 4
|
||
]
|
||
|
||
# But navigation uses local coordinates
|
||
current_position = (10.5, 3.2, -5.0) # NED, meters
|
||
```
|
||
|
||
**Breach Actions:**
|
||
- `RTL` - Return to local origin
|
||
- `LAND` - Land immediately
|
||
- `HOLD` - Hold position
|
||
|
||
## Testing GPS-Denied Mode
|
||
|
||
### 1. Verify EKF Source
|
||
|
||
In MAVProxy:
|
||
```
|
||
param show EK3_SRC*
|
||
```
|
||
|
||
Should show:
|
||
```
|
||
EK3_SRC1_POSXY 6.0
|
||
EK3_SRC1_VELXY 6.0
|
||
```
|
||
|
||
### 2. Check Vision Input
|
||
|
||
```bash
|
||
ros2 topic echo /uav/visual_odometry/pose
|
||
```
|
||
|
||
Should show updating position.
|
||
|
||
### 3. Monitor EKF Status
|
||
|
||
```bash
|
||
ros2 topic echo /uav/mavros/state
|
||
```
|
||
|
||
Should show `connected: true` and `mode: GUIDED`.
|