129 lines
3.0 KiB
Markdown
129 lines
3.0 KiB
Markdown
# GPS-Denied Navigation
|
|
|
|
How the system navigates without GPS.
|
|
|
|
## Principle
|
|
|
|
All navigation uses relative positioning from visual sensors. GPS is only used for geofencing (safety boundaries).
|
|
|
|
| Function | GPS Used? |
|
|
|----------|-----------|
|
|
| Position estimation | No - visual odometry |
|
|
| Waypoint navigation | No - local coordinates |
|
|
| Velocity control | No - optical flow |
|
|
| Geofencing | Yes - safety only |
|
|
|
|
## Position Estimation
|
|
|
|
### Visual Odometry
|
|
|
|
1. Detect features in camera image (ORB, SIFT)
|
|
2. Match features between consecutive frames
|
|
3. Estimate camera motion from feature displacement
|
|
4. Accumulate motion into position estimate
|
|
|
|
### Optical Flow
|
|
|
|
1. Capture ground images from downward camera
|
|
2. Measure pixel displacement between frames
|
|
3. Convert to velocity using altitude
|
|
4. Integrate for position
|
|
|
|
### Sensor Fusion
|
|
|
|
Extended Kalman Filter combines:
|
|
- Visual odometry (position)
|
|
- Optical flow (velocity)
|
|
- IMU (acceleration, rotation)
|
|
- Barometer (altitude)
|
|
|
|
Output: Full 6-DOF pose estimate
|
|
|
|
## ArduPilot Configuration
|
|
|
|
Key parameters for GPS-denied operation:
|
|
|
|
```
|
|
# EKF Source Configuration
|
|
EK3_SRC1_POSXY = 6 # External Nav for position
|
|
EK3_SRC1_VELXY = 6 # External Nav for velocity
|
|
EK3_SRC1_POSZ = 1 # Barometer for altitude
|
|
|
|
# Disable GPS for navigation
|
|
GPS_TYPE = 0 # No GPS (or keep for geofence)
|
|
|
|
# Enable external navigation
|
|
VISO_TYPE = 1 # Enable visual odometry input
|
|
|
|
# Arming checks
|
|
ARMING_CHECK = 0 # Disable pre-arm checks (for testing)
|
|
```
|
|
|
|
See `config/ardupilot_gps_denied.parm` for complete parameters.
|
|
|
|
## Sending Position to ArduPilot
|
|
|
|
Visual odometry sends position via MAVLink:
|
|
|
|
```python
|
|
# VISION_POSITION_ESTIMATE message
|
|
msg = mavutil.mavlink.MAVLink_vision_position_estimate_message(
|
|
usec=timestamp_us,
|
|
x=position_x, # meters, NED frame
|
|
y=position_y,
|
|
z=position_z,
|
|
roll=roll, # radians
|
|
pitch=pitch,
|
|
yaw=yaw
|
|
)
|
|
```
|
|
|
|
## Drift Mitigation
|
|
|
|
Visual odometry accumulates drift over time. Strategies:
|
|
|
|
1. **Loop Closure**: Recognize previously visited locations
|
|
2. **Landmark Matching**: Use known visual markers
|
|
3. **Multi-Sensor Fusion**: Weight sensors by confidence
|
|
4. **Periodic Reset**: Return to known position
|
|
|
|
## Geofencing
|
|
|
|
GPS is only used for safety boundaries:
|
|
|
|
```yaml
|
|
geofence:
|
|
enabled: true
|
|
use_gps: true
|
|
fence_type: polygon
|
|
action: RTL
|
|
max_altitude: 50
|
|
```
|
|
|
|
If drone crosses boundary, triggers return-to-launch.
|
|
|
|
## Coordinate System
|
|
|
|
All waypoints use local NED coordinates:
|
|
- X: North (meters from origin)
|
|
- Y: East (meters from origin)
|
|
- Z: Down (negative for altitude)
|
|
|
|
Example mission:
|
|
```python
|
|
waypoints = [
|
|
{"x": 0, "y": 0, "z": -5}, # Takeoff to 5m
|
|
{"x": 10, "y": 0, "z": -5}, # 10m north
|
|
{"x": 10, "y": 10, "z": -5}, # 10m east
|
|
{"x": 0, "y": 0, "z": -5}, # Return
|
|
{"x": 0, "y": 0, "z": 0}, # Land
|
|
]
|
|
```
|
|
|
|
## Limitations
|
|
|
|
- Drift accumulates over distance/time
|
|
- Requires visual features (fails in featureless environments)
|
|
- Requires sufficient lighting
|
|
- Performance degrades with fast motion or blur
|