Spearhead UAV Simulation — System Documentation

Overview

spearhead_sim_quart.py simulates a 120-second VTOL-to-fixed-wing transition flight using quaternion-based 6-DOF rigid body dynamics with nested PID control. The vehicle is a quadrotor-wing hybrid with four vertical lift rotors (m1–m4), one pusher motor (m5), and three aerodynamic surfaces (left elevon, right elevon, rudder).

---

Vehicle Parameters

Parameter	Value
Mass	20 kg
Ixx / Iyy / Izz	8.734 / 5.592 / 13.623 kg·m²
Wing area S	2.24 m²
Mean chord C	0.32 m
Rotor arm L1	1.65 m
Pusher arm L2	1.425 m

Motor thrust/torque coefficients:

• Lift rotors (K[0]): 1.123×10⁻⁶ N·s²/rad²
• Pusher thrust (K[1]): 2.25×10⁻⁶ N·s²/rad²
• Lift rotor reaction torque (K[2]): 7.708×10⁻⁷ N·m·s²/rad²
• Pusher reaction torque (K[3]): 18.708×10⁻⁷ N·m·s²/rad²

First-order motor dynamics (τ = 1/44.22 s for rotors/pusher, 1/20 s for servos):

• Steady-state rotor speed: w_ss = 8.18 × cmd (rad/s/cmd unit)
• Steady-state pusher speed: w_ss = 7.02 × cmd

---

State Vector (21 elements)

Indices	Variables	Description
0–2	u, v, w	Body-frame velocities (m/s)
3–5	p, q, r	Body angular rates (rad/s)
6–8	x, y, z	NED position (m)
9–12	q0–q3	Attitude quaternion
13–17	w1–w5	Rotor/pusher angular speeds (rad/s)
18–20	dl, dr, drd	Elevon/rudder deflection angles (deg)

NED convention: z positive downward. ref_alt = -10 m = 10 m AGL.

---

Flight Phases

Phase	Time	Description
SPINUP	0–5 s	Motors ramp linearly from 0 → W_HOVER (808 cmd). No control, no pusher.
HOVER	5–30 s	Altitude feedback active. Pusher off. Target: 10 m AGL, 0 m/s forward.
TRANSITION	30–60 s	Pusher ramps open-loop 0→320 cmd. Speed builds toward 20 m/s.
FWD_FLIGHT	60–120 s	Closed-loop speed control via pusher PID. Surfaces take over attitude.

---

Aerodynamic Model

Coefficients [CFX, CFY, CFZ, CMX, CMY, CMZ] are computed from three sources, summed:

1. Body ADB (adb_w_hat.txt): 56×9×6 table, indexed by β (56 breakpoints, ±180°) and α (9th-order polynomial, clamped to ±30°).
2. Left elevon (le_w_hat.txt): 9×6 polynomial in deflection angle.
3. Right elevon (re_w_hat.txt): 9×6 polynomial in deflection angle.
4. Rudder (rudder_w_hat.txt): 9×6 polynomial in deflection angle.

Key values at α=β=0:

• CFX = +0.00374 (constant forward aero force across 0–15° α)
• CFZ = −0.04888 (lift; wing fully supports M·g at ~54 m/s)

Dynamic pressure: qS = ½ρV²·S

---

Dynamics Model (spearhead_model_quat)

Translational equations of motion (body frame):

du/dt = (FGx + FAx + FTx)/M − q·w + r·v
dv/dt = (FGy + FAy + FTy)/M − r·u + p·w
dw/dt = (FGz + FAz + FTz)/M − p·v + q·u

Rotational equations (Euler's rigid body):

dp/dt = (LA + LT − q·r·(Izz−Iyy)) / Ixx
dq/dt = (MA + MT − p·r·(Ixx−Izz)) / Iyy
dr/dt = (NA + NT − p·q·(Iyy−Ixx)) / Izz

Force/moment sources:

• FG — gravity rotated into body frame via quaternion
• FA — aerodynamic forces (qS · CF*)
• FT — thrust: [F5, 0, −(F1+F2+F3+F4)] (pusher forward, rotors up in NED)
• LT = L1·((F1+F3)−(F2+F4)) + Tau5 — roll moment (rotors + pusher reaction)
• MT = L2·((F1+F2)−(F3+F4)) — pitch moment
• NT = Tau1−Tau2−Tau3+Tau4 — yaw moment

Numerical integration: RK4, dt = 0.001 s.

---

Control Architecture

Layer 1 — Altitude (outer loop)

AltitudeController: NED altitude error → desired vertical velocity.

• Output clamped to ±1.5 m/s (hover) or ±0.6 m/s (forward flight).

Layer 2 — Velocity (middle loop)

VelocityController:

• vert_pid: vertical velocity error → vertical thrust correction (cmd units)
• fwd_pid: forward velocity error → pusher throttle (cmd units). Active in FWD only.
• fwd_pitch_pid: forward velocity error → pitch reference (rad). Active in transition only.

Layer 3 — Attitude (middle loop)

Two parallel controllers, blended by phase:

• AttitudeController (hover): angle error → desired body rate. Used for m1–m4 differential.
• ForwardAttitudeController (FWD): same structure, different gains tuned for aerodynamic control effectiveness at V=15 m/s.

Layer 4 — Rate (inner loop)

Two parallel controllers:

• RateController (hover): body rate error → motor differential command (cmd units)
• ForwardRateController (FWD): body rate error → elevon/rudder deflection (deg). Includes V² gain scheduling: commands divided by (vx/15)².

---

Actuator Mixing

Hover rotors (m1–m4):

m1 = base + ( roll + pitch + yaw)   # front-right
m2 = base + (−roll + pitch − yaw)   # front-left
m3 = base + ( roll − pitch − yaw)   # rear-right
m4 = base + (−roll − pitch + yaw)   # rear-left

All commands clipped 0–1000. base_throttle = hover_cmd_frac × (W_HOVER + thrust_cmd), floored at 100 cmd in forward flight to preserve differential authority.

Elevons (left/right) and rudder:

servo_le  = surf_blend × (pitch_cmd_f + roll_cmd_f)   clipped ±20°
servo_re  = surf_blend × (pitch_cmd_f − roll_cmd_f)   clipped ±20°
servo_rud = surf_blend × yaw_cmd_f                    clipped ±20°

---

Key Control Features

Wing Lift Fraction

As forward speed increases, the wing progressively offloads the rotors:

wing_lift_frac = clip(CFZ0 · ½ρ · vx² · S / (M·g), 0, 1)   [CFZ0 = 0.04888]
hover_cmd_frac = sqrt(1 − wing_lift_frac)

The altitude demand is split: rotors handle hover_cmd_frac, wing angle-of-attack handles the remainder via ref_pitch_alt = −desired_vz · wing_lift_frac / max(vx, 15).

Speed-Based Pitch Reference (FWD)

At cruise, the small forward aero force (CFX) is balanced by gravity drag at ~0.6° pitch, requiring zero pusher at equilibrium:

pitch_cruise_eq = (CFX · qS(ref_vx) · S) / (M·g)   ≈ 0.600° at 20 m/s
pitch_spd_fb    = 0.005 · (vx − ref_vx)             [rad per m/s overshoot]
ref_pitch_acc   = clip(pitch_cruise_eq + pitch_spd_fb, 0, 0.20 rad)

When vx exceeds target, increased pitch raises gravity drag and decelerates without touching the pusher.

Pusher Reaction Torque Feedforward

The pusher imparts a roll moment Tau5 = K[3]·w5². A feedforward term cancels it proactively:

roll_ff = −Tau5 / (L1 · K[0] · 8 · 8.18² · base_throttle)

Surface Blend Gate

Forward surfaces activate gradually above 5 m/s:

surf_blend = clip((vx − 5) / (ref_fwd_vel − 5), 0, 1)

Hover pitch/yaw authority fades out as 1 − surf_blend to prevent authority gap at FWD entry.

Integrator Management

On FWD entry: altitude PID, vertical velocity PID, and forward PID integrals are zeroed (not full reset — preserves prev_error to avoid Kd derivative spikes). Forward surface PIDs are reset when surf_blend > 0.10.

---

Outputs

Each run produces two files in logs/:

• .log — human-readable: parameters, PID gains, phase events, per-phase statistics, end state
• .csv — 39-column time-series at 10 Hz: position, velocity, attitude, rates, α/β, motor speeds, surface deflections, actuator commands, blend factors, errors

---

File Dependencies

File	Contents
spearhead_sim_quart.py	Main simulation
adb_w_hat.txt	Body aerodynamic database (56×9×6)
le_w_hat.txt	Left elevon polynomial (9×6)
re_w_hat.txt	Right elevon polynomial (9×6)
rudder_w_hat.txt	Rudder polynomial (9×6)
logs/flight_*.log	Run logs
logs/flight_*.csv	Run time-series data