Controller Update
This commit is contained in:
@@ -1,120 +1,177 @@
|
||||
# Architecture
|
||||
|
||||
System architecture for GPS-denied UAV/UGV navigation.
|
||||
# System Architecture
|
||||
|
||||
## Overview
|
||||
|
||||
The system uses vision-based navigation with GPS reserved only for geofencing.
|
||||
|
||||
```
|
||||
Vision Sensors (Cameras)
|
||||
|
|
||||
v
|
||||
Visual Odometry & Optical Flow
|
||||
|
|
||||
v
|
||||
Position Estimator (EKF)
|
||||
|
|
||||
v
|
||||
ArduPilot Flight Controller
|
||||
|
|
||||
v
|
||||
Motor Control
|
||||
┌─────────────────────────────────────────────────────────────────┐
|
||||
│ Simulation │
|
||||
├───────────────────┬─────────────────────┬───────────────────────┤
|
||||
│ Gazebo Harmonic │ ArduPilot SITL │ ROS 2 Nodes │
|
||||
│ (Physics/3D) │ (Flight Control) │ (Perception/Nav) │
|
||||
└───────────────────┴─────────────────────┴───────────────────────┘
|
||||
│ │ │
|
||||
└────────────────────┼──────────────────────┘
|
||||
│
|
||||
┌─────────┴─────────┐
|
||||
│ MAVLink/MAVROS │
|
||||
└───────────────────┘
|
||||
```
|
||||
|
||||
## Components
|
||||
|
||||
### Perception
|
||||
### 1. Gazebo Harmonic
|
||||
|
||||
- **Forward Camera**: Visual odometry, feature tracking
|
||||
- **Downward Camera**: Optical flow, ground plane detection
|
||||
- **IMU**: Angular velocity, acceleration
|
||||
**Role:** 3D simulation, physics, sensors
|
||||
|
||||
### Localization
|
||||
- **World simulation**: Ground plane, lighting, physics
|
||||
- **UAV model**: Iris quadcopter with cameras
|
||||
- **UGV model**: Differential drive rover
|
||||
- **Sensors**: Cameras, IMU, rangefinder
|
||||
|
||||
- **Visual Odometry**: Estimates motion from camera images
|
||||
- **Optical Flow**: Velocity estimation from downward camera
|
||||
- **EKF Fusion**: Combines all sensor inputs into position estimate
|
||||
### 2. ArduPilot SITL
|
||||
|
||||
### Navigation
|
||||
**Role:** Flight controller simulation
|
||||
|
||||
- **Waypoint Navigation**: Relative coordinates (meters from origin)
|
||||
- **Path Planning**: Collision-free paths using local map
|
||||
- **Position Hold**: Maintain position using vision
|
||||
- **EKF3**: State estimation using external vision
|
||||
- **Flight modes**: GUIDED, LOITER, RTL, LAND
|
||||
- **Motor mixing**: Quadcopter dynamics
|
||||
- **Failsafe**: Battery, geofence, communication
|
||||
|
||||
### Control
|
||||
**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
|
||||
```
|
||||
|
||||
- **ArduPilot**: Flight controller firmware
|
||||
- **MAVROS**: ROS interface to ArduPilot
|
||||
- **Velocity/Position Control**: Low-level motor commands
|
||||
### 3. ROS 2 Nodes
|
||||
|
||||
### Safety
|
||||
#### Vision Pipeline
|
||||
|
||||
- **Geofencing**: GPS-based boundary checking (safety only)
|
||||
- **Altitude Limits**: Maximum flight ceiling
|
||||
- **Failsafe**: RTL on signal loss or boundary breach
|
||||
```
|
||||
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 |
|
||||
|-------|-------------|
|
||||
| body | Attached to vehicle, X forward, Y right, Z down |
|
||||
| odom | Origin at takeoff, accumulates drift |
|
||||
| map | Fixed world frame (may be corrected) |
|
||||
| `odom` | Local origin (takeoff point) |
|
||||
| `base_link` | Vehicle body frame |
|
||||
| `map` | World frame (aligned with odom) |
|
||||
|
||||
All navigation commands use local coordinates relative to takeoff point.
|
||||
**NED Convention:**
|
||||
- X = North (forward)
|
||||
- Y = East (right)
|
||||
- Z = Down (negative altitude)
|
||||
|
||||
## Data Flow
|
||||
## File Structure
|
||||
|
||||
```
|
||||
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
|
||||
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
|
||||
```
|
||||
|
||||
## ArduPilot Integration
|
||||
## Configuration
|
||||
|
||||
ArduPilot receives external position via MAVLink:
|
||||
### ArduPilot EKF Sources
|
||||
|
||||
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)
|
||||
```yaml
|
||||
EK3_SRC1_POSXY: 6 # External Nav
|
||||
EK3_SRC1_POSZ: 1 # Barometer
|
||||
EK3_SRC1_VELXY: 6 # External Nav
|
||||
EK3_SRC1_YAW: 6 # External Nav
|
||||
```
|
||||
|
||||
## Geofencing
|
||||
### Sensor Weights
|
||||
|
||||
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
|
||||
|
||||
| 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 |
|
||||
```yaml
|
||||
vo_weight: 0.6 # Visual odometry
|
||||
optical_flow: 0.3 # Optical flow
|
||||
imu: 0.1 # IMU integration
|
||||
```
|
||||
|
||||
Reference in New Issue
Block a user