Model Overview
This model uses an analogy to electrostatic forces to guide a drone towards a goal while avoiding obstacles. The drone is treated as a charged particle influenced by attractive and repulsive forces.
Definitions
- \(d\vec{p}\): Position vector of the drone
- \(d\vec{v} = \dfrac{d\vec{p}}{dt}\): Velocity vector of the drone
- \(\vec{g}\): Position vector of the goal
- \(\vec{o_i}\): Position vector of the center of obstacle \(i\) (assumed spherical)
- \(n_{obs}\): Number of obstacles
- \(\vec{r}_g = \vec{p} - \vec{g}\) (goal to drone)
- \(\vec{r}_i = \vec{p} - \vec{o_i}\) (obstacle to drone)
- \(\forall \vec{v} \neq \vec{0}, \hat{v} = \dfrac{\vec{v}}{\lVert\vec{v}\rVert}\). \(\hat{0} = \vec{0}\). \(\hat{v}\) is dimensionless, even if \(\vec{v}\) has units.
Force Equations
Total Force
\[
\vec{F}_{total} = \vec{F}_{goal} + \vec{F}_{\text{obs, squash}} + \vec{F}_{damping}
\]
Goal Force
\[
\vec{F}_{goal} = \left( -F_{\text{goal, const}} + \frac{k_e \cdot Q_{drone} \cdot
Q_{\text{goal}}}{\lVert\vec{r}_g\rVert^2 + \varepsilon_{\text{goal}}^2} \right) \hat{r}_g
\]
Notes:
- The goal force is always attractive, pulling the drone towards the goal, due to \(Q_{goal} < 0\).
- \(k_e = 1 \left(N \cdot m^2 \cdot C^{-2}\right)\), a Coulomb's constant analog, is included for dimensional consistency.
- \(Q_{drone} = 1 \left(C\right)\) is the drone's charge, which is constant.
Obstacle Force
\[
\vec{F}_{\text{obs},i} = \left( \frac{ k_e \cdot Q_{drone} \cdot Q_i }{ \lVert\vec{r}_i\rVert^2 +
\varepsilon_{\text{obs}}^2 } \right) \hat{r}_i
\]
Raw and Squashed Obstacle Force
\[
\vec{F}_{\text{obs, raw}} = \sum_{i=1}^{n_{\text{obs}}}{\vec{F}_{\text{obs},i}}
\]
\[
\vec{F}_{\text{obs, squash}} = F_{\text{fac,squash}} \cdot \tanh\left( \frac{ \lVert \vec{F}_{\text{obs,
raw}} \rVert }{ F_{\text{sat}} } \right) \cdot \hat{F}_{\text{obs, raw}}
\]
Damping Force
\[
\vec{F}_{damping} = -K_{damping} \cdot \vec{v}
\]
From Guidance Field to Desired Velocity
Let \(\vec{F}_{\text{field}}\) define the direction in which the drone "should" move:
Compute the unit direction:
\[
\hat{d} = \begin{cases}
\dfrac{\vec{F}_{\text{field}}}{\Vert \vec{F}_{\text{field}} \Vert}, & \text{if } \vec{F}_{\text{field}}
\neq \vec{0} \\
\vec{0}, & \text{otherwise}
\end{cases}
\]
Fix a target scalar speed \(v_{\text{ideal}} > 0\) (constant in the code) and a slow-down distance \(d_{\text{slow}} = 4 \, \text{m}\).
Define the desired velocity vector:
\[
\vec{v}_{\text{set}} = v_{\text{ideal}} \cdot \max\left(\frac{1}{3},
\frac{\lVert\vec{r}_g\rVert}{d_{\text{slow}}}\right) \cdot \hat{d}
\]
This scales down the velocity as the drone approaches the goal, ensuring smooth arrival.
Control System
After computing \(\vec{v}_{\text{set}}\), the control system is designed to resemble the PX4 control architecture to allow for meaningful calibration results. The system follows this cascade structure:
- Velocity PID Controller - Computes desired acceleration setpoint from velocity signal
- Thrust Vector Computation - Converts acceleration to thrust force (world frame)
- Thrust Command Extraction - Projects thrust along body z-axis and normalizes
- Desired Attitude Computation - Derives quaternion from thrust vector and velocity direction
- Attitude Error to Body-Rate Setpoint - P-controller on attitude error quaternion
- Body-Rate Clamping - Saturates angular velocity setpoints to maximum limits
- Noise-Filtered Angular Velocity Derivative - 2nd-order Butterworth low-pass filter on derivative of angular velocity measurement
- Torque PID Controller - Computes body torque commands from body rate signal
- Yaw Torque Filtering - 1st-order low-pass on yaw torque output
- Force and Torque Application - Converts normalized commands to Newton forces/torques in simulation
System Invariants
VFF Guidance Constants
- Coulomb's constant (analog): \(k_e = 1 \, \text{N} \cdot \text{m}^2 \cdot \text{C}^{-2}\)
- Drone charge: \(Q_{drone} = 1 \, \text{C}\)
- Obstacle charges: \(Q_i \in [0, +1] \, \text{C}\)
- Ideal velocity: \(v_{\text{ideal}} = 5.0 \, \text{m/s}\)
- Slow-down distance: \(d_{\text{slow}} = 4.0 \, \text{m}\)
Physical Properties
- Drone mass: \(m = 2.064 \, \text{kg}\)
- Drone inertia: \(I_x = 0.034 \, \text{kg} \cdot \text{m}^2, \, I_y = 0.035 \, \text{kg} \cdot \text{m}^2, \, I_z = 0.062 \, \text{kg} \cdot \text{m}^2\)
- Drone shape (box): \(0.6 \times 0.6 \times 0.27 \, \text{m}\)
- Gravity: \(g = 9.81 \, \text{m/s}^2\)
Actuator Limits
- Maximum thrust: \(T_{\max} = 24.0 \, \text{N}\)
- Roll arm length: \(L_{\text{roll}} = 0.087 \, \text{m}\)
- Pitch arm length: \(L_{\text{pitch}} = 0.087 \, \text{m}\)
- Maximum yaw moment: \(Q_{\max} = 0.192 \, \text{N} \cdot \text{m}\)
Control System Fixed Parameters
- Simulation timestep: \(\Delta t = 1/60 \, \text{s} \approx 0.0167 \, \text{s}\)
- Yaw torque filter cutoff: \(f_{c,\text{yaw}} = 2.0 \, \text{Hz}\)
- IMU gyro filter cutoff: \(f_{c,\text{gyro}} = 20.0 \, \text{Hz}\)
- Max roll rate: \(\omega_{\max,\phi} = 220.0 \, \text{rad/s}\)
- Max pitch rate: \(\omega_{\max,\theta} = 220.0 \, \text{rad/s}\)
- Max yaw rate: \(\omega_{\max,\psi} = 200.0 \, \text{rad/s}\)
- Roll rate integrator limit: \(I_{\max,\phi} = 0.3\)
- Pitch rate integrator limit: \(I_{\max,\theta} = 0.3\)
- Yaw rate integrator limit: \(I_{\max,\psi} = 0.3\)
Parameters for Calibration
These parameters are determined by the simulation calibration program.
VFF Guidance Parameters
| Parameter | Description |
|---|---|
| \(Q_{goal}\) | Goal charge |
| \(F_{goal, const}\) | Goal constant attraction |
| \(F_{\text{sat}}\) | Obstacle total saturation scale |
| \(F_{\text{fac,squash}}\) | Obstacle total factor after squashing |
| \(Q_{\text{vertical,obs}}\) | Vertical obstacle charge modifier |
Velocity PID Parameters
| Parameter | Description |
|---|---|
| \(K_x, K_y, K_z\) | Velocity P gains (x, y, z axis) |
| \(K_{i,x}, K_{i,y}, K_{i,z}\) | Velocity I gains (x, y, z axis) |
| \(K_{d,x}, K_{d,y}, K_{d,z}\) | Velocity D gains (x, y, z axis) |
Attitude Controller Parameters
| Parameter | Description |
|---|---|
| MC_ROLL_P | Roll attitude gain |
| MC_PITCH_P | Pitch attitude gain |
| MC_YAW_P | Yaw attitude gain |
Rate Controller Parameters
| Parameter | Description |
|---|---|
| MC_ROLLRATE_P, MC_ROLLRATE_I, MC_ROLLRATE_D | Roll rate PID gains |
| MC_ROLLRATE_FF, MC_ROLLRATE_K | Roll rate feedforward and global gain |
| MC_PITCHRATE_P, MC_PITCHRATE_I, MC_PITCHRATE_D | Pitch rate PID gains |
| MC_PITCHRATE_FF, MC_PITCHRATE_K | Pitch rate feedforward and global gain |
| MC_YAWRATE_P, MC_YAWRATE_I, MC_YAWRATE_D | Yaw rate PID gains |
| MC_YAWRATE_FF, MC_YAWRATE_K | Yaw rate feedforward and global gain |
Implementation Notes
- No singularities: Both goal and obstacles use \(\varepsilon\) to avoid \(1/r^2\) blow-ups.
- Dimensionless tanh: \(\dfrac{\lVert \vec{F}_{\text{obs, raw}} \rVert}{F_{\text{sat}}}\) is unitless; \(F_{\text{fac,squash}}\) carries the resulting Newton scale.
- Performance optimization: For \(\lvert \tanh(x) - x \rvert < 0.01\) when \(x \in [-0.3, 0.3]\), we skip squashing to improve performance.
- Velocity control: The system uses VFF guidance for direction and speed, followed by a PX4-like cascaded control architecture for precise velocity and attitude tracking.
- Slow-down behavior: The velocity magnitude scales linearly with distance to goal (below \(d_{\text{slow}}\)), with a minimum of \(v_{\text{ideal}}/3\).