EOM 6DOF Custom Variable Mass Wind Angles — Robotics/Equations Of Motion

EOM 6DOF Custom Variable Mass Wind Angles

Robotics / Equations Of Motion

The six-degree-of-freedom equations of motion of a rigid body whose mass and inertia arrive on wires, carried in wind axes – the airspeed V with the incidence α and the sideslip β – with the attitude as the wind angles bank μ, flight-path angle γ and heading χ. With the applied force F in wind axes, a = F/m, the body rates ω = [p; q; r] and DCM_wb the body-to-wind rotation of (α, β):

• V' = a_x
• β' = p·sin α − r·cos α + a_y/V
• α' = q − (p·cos α + r·sin α)·tan β + a_z/(V·cos β)
• [p_w; q_w; r_w] = DCM_wb·[p − β'·sin α; q − α'; r + β'·cos α]
• μ' = p_w + (q_w·sin μ + r_w·cos μ)·tan γ, γ' = q_w·cos μ − r_w·sin μ, χ' = (q_w·sin μ + r_w·cos μ)/cos γ
• ω' = I^−T(M − dI/dt·ω − ω × (Iω))
• X_e' = DCM_we^T·[V; 0; 0]

Ports

• F – the applied force in wind axes, [3,1].
• M – the applied moment about the centre of gravity in body axes, [3,1].
• m – the mass, a scalar [1,1] (it divides, so it must be nonzero).
• I_dot – the rate of change of the inertia tensor, [3,3].
• I – the inertia tensor in body axes, [3,3] with a nonzero determinant.
• Ve – the velocity in the flat-earth frame, DCM_we^T·[V; 0; 0], [3,1].
• Xe – the position in the flat-earth frame, [3,1].
• Wind – the wind angles [μ; γ; χ] in radians, [3,1].
• DCMwe – the direction cosine matrix from the flat-earth frame to wind axes, [3,3].
• Vw – the velocity in wind axes, [V; 0; 0], [3,1].
• alpha_beta – [α; β] in radians, [2,1].
• alpha_beta_dot – [α'; β'] in rad/s, [2,1].
• wb – the body rates [p; q; r] in rad/s, [3,1].
• wb_dot – the angular accelerations [p'; q'; r'] in rad/s², [3,1].
• Ab – the acceleration in body axes, V_b × ω + DCM_wb^T·a with V_b = DCM_wb^T·[V; 0; 0], [3,1].

Parameters

• Units – Metric (MKS) (the default) or English (velocity in ft/s). The two are the same arithmetic, so the choice only says which units the numbers are in.
• Initial Position [Xe Ye Ze] – three values. Defaults to [0 0 0].
• Initial Velocity [V alpha beta] – the initial airspeed, incidence and sideslip (radians), three values. Defaults to [0 0 0]; the airspeed divides, so a run needs a nonzero one.
• Initial Wind Angles [mu gamma chi] – the initial bank, flight-path and heading angles in radians, three values, with |γ| below 90°. Defaults to [0 0 0].
• Initial Body Rates [p q r] – in rad/s, three values. Defaults to [0 0 0].
• Integration Substeps – M, how many fourth-order Runge-Kutta steps a sample is integrated with on the discrete solver and in exported code, a whole number of 1 or more. Defaults to 100. No Simulink counterpart.
• Sampling Time (s) – zero or less inherits the solver's rate; a positive value runs the block at that period.

Code export

All ten targets: Python, MATLAB, Java, Rust, C, C++, VHDL, Verilog, SystemVerilog and PLC Structured Text. A core holds the twelve states, publishes the ten outputs from them and the current inputs, and integrates one sample with the inputs held and M Runge-Kutta substeps – with M = 100, the same computation as Simulink's fixed-step ode4 at one hundredth of the sample time. The inertia is inverted in closed form at every derivative evaluation, since it arrives on a wire.

The three HDL targets are simulation-only real arithmetic, quantized at the port: a sine of a state has no Q16.16 form. The cores simulate correctly and are not offered as synthesizable.

Simulink bridge

Import and export, mapped to Aerospace Blockset's aerolib6dof2/Custom Variable Mass 6DOF Wind (Wind Angles): Units → units (Metric (MKS) 1:1, English (velocity in ft/s) ↔ English (Velocity in ft/s)), Initial Position [Xe Ye Ze] → xme_0, Initial Velocity [V alpha beta] → Vm_0, Initial Wind Angles [mu gamma chi] → wind_0 and Initial Body Rates [p q r] → pm_0. Always emitted with mtype = Custom Variable, rep = Wind Angles and vre_flag and abi_flag off: each of those moves that block's port list, and this block has one. The dialog's mass and inertia values are not read at this mass type – they are on the wires – and the knots unit system is not offered, because it converts velocities inside the integration; an imported one is reported. "Sampling Time (s)" does not cross: the Simulink block is continuous and defines no SampleTime.

Notes

• Stateful, continuous and nonlinear: twelve continuous states. Three outputs – alpha_beta_dot, wb_dot and Ab – read the inputs, so the block has direct feedthrough, as the Simulink block does.
• The angular acceleration uses the transpose of the inertia's inverse, I^−T, as the Simulink block does (it right-divides a row by I). For a symmetric inertia – any physical one – that is I⁻¹; for an asymmetric matrix the two differ.
• The block does not check that the I and I_dot wires agree, and a singular I or a zero m divides by zero, in this block as in Simulink's.
• V = 0, β = ±90° and γ = ±90° are singular, as on the fixed-mass block.