Commit 0cdddb26 authored by mmroma's avatar mmroma
Browse files

Added drag forces

parent 424758a3
function [params] = setUpParameters_sphere_1_stationary()
function [params] = setUpParameters_sphere_1()
%setUpParameters
% Stationary Test
......@@ -58,7 +58,6 @@ function [params] = setUpParameters_sphere_1_stationary()
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.0025; % Simulation Step Size (s)
......
function [params] = setUpParameters_sphere_2()
%setUpParameters
% Stationary Test with Wind Added
%% Load Payload (sphere in this case)
params.payload = parameters_Payload();
% params.payload.x0(1:3) = [0;0;sqrt(3)/2];
params.payload.x0(1:3) = [0;0;0];
r = params.payload.diameter / 2;
%% Load Multirotors
params.num_vehicles = 4;
L_all = 1.5;
height_all = -1.5;
% Q1
i = 1;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [0.5;-0.5; height_all];
params.multirotors{i}.xd(1:3) = [0.5;-0.5; height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',-pi/4) *...
makehgtform('yrotate',pi/4) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
% Q2
i = 2;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [0.5;0.5;height_all];
params.multirotors{i}.xd(1:3) = [0.5;0.5;height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',pi/4) *...
makehgtform('yrotate',pi/4) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
% Q3
i = 3;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [-0.5;-0.5;height_all];
params.multirotors{i}.xd(1:3) = [-0.5;-0.5;height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',-3*pi/4) *...
makehgtform('yrotate',pi/4) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
% Q4
i = 4;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [-0.5;0.5;height_all];
params.multirotors{i}.xd(1:3) = [-0.5;0.5;height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',3*pi/4) *...
makehgtform('yrotate',pi/4) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
%% Wind Parameters
params.wind.speed = 0; % (m/s)
params.wind.yaw = 0; % (rad). 0 = +x, pi/2 = +y
params.wind.rho_f = 1.225; % (kg/m^3)
params.wind.mu = 1.802e-5; % (kg/ms)
params.wind.load_d = params.payload.diameter; % m
params.wind.C_D = @(Re) C_D_sphere(Re); % Sphere Drag Model
c_bar_d = 0.04; % Quad Lumped Drag
params.wind.C_bar_d = [c_bar_d, 0, 0;
0, c_bar_d, 0;
0, 0, 0];
params.wind.R_W_G = makehgtform('zrotate',params.wind.yaw);
params.wind.R_W_G = params.wind.R_W_G(1:3,1:3);
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.0025; % Simulation Step Size (s)
params.anim_FPS = 60; % Animation FPS
%% Sensor Noise
params.noiseSTD = 0; %0.1; % 0-mean White Gaussian Noise (Sensor Noise)
end
function [C_D] = C_D_cube(Re)
%C_D_cube
phi = (pi/6)^(1/3);
phi_perp = (9*pi/16)^(1/3);
phi_par = ((9*pi/16)^(1/3) ) / (3 - (4/pi));
C_D = (8./Re) .* (1./ sqrt(phi_par))...
+ (16./Re) .* (1./ sqrt(phi))...
+ (3./sqrt(Re)) .* (1./ phi.^(3./4))...
+ 0.421.^(0.4 .* (- log(phi) ).^2 )...
.* (1 ./ phi_perp);
end
function [C_D] = C_D_sphere(Re)
%C_D_sphere
C_D = (24 ./ Re) .* (1 + 0.15 .* Re .^ 0.681) + 0.407 ./ (1 + 8710./Re);
end
function [Re] = reynolds_num(v_w, params)
%reynolds_num
Re = params.wind.rho_f * v_w * params.wind.load_d / params.wind.mu;
end
function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,U,params)
%systemModel Implements the System Model Dynamics for a multicopter with a
%slung load. Computes the derivative of the state given the state
% Inputs:
% x: state (num_vehicles*12 + 12x1)
% U: input (params.numRotors x 1)
% Outputs:
% xdot: state derivative (num_vehicles*12 + 12 x 1)
% F_nets: net forces (num_vehicles*3 + 3 x 1)
% Tao_nets: net torques (num_vehicles*3 + 3 x 1)
% F_tethers: tether forces (num_vehicles*3 x 1)
% F_drags: drag forces (num_vehicles*3 + 3 x 1)
% Tether_states: (num_vehicles*2 x 1)
%% Setup output variables
xdot = [];
F_nets = [];
Tao_nets = [];
F_tethers = [];
F_drags = [];
Tether_states = [];
%% 0) Constrain Input
U = min(U,1);
U = max(U,0);
%% 1) Grab Payload Information first
m_load = params.payload.m;
load_Jx = params.payload.Jx;
load_Jy = params.payload.Jy;
load_Jz = params.payload.Jz;
load_x = X(params.num_vehicles*12 + 1);
load_y = X(params.num_vehicles*12 + 2);
load_z = X(params.num_vehicles*12 + 3);
load_u = X(params.num_vehicles*12 + 4);
load_v = X(params.num_vehicles*12 + 5);
load_w = X(params.num_vehicles*12 + 6);
load_phi = X(params.num_vehicles*12 + 7);
load_theta = X(params.num_vehicles*12 + 8);
load_psi = X(params.num_vehicles*12 + 9);
load_p = X(params.num_vehicles*12 + 10);
load_q = X(params.num_vehicles*12 + 11);
load_r = X(params.num_vehicles*12 + 12);
load_omega = [load_p; load_q; load_r];
R_P_G = rpy_to_R(load_phi, load_theta, load_psi);
load_pos_d = R_P_G * [load_u; load_v; load_w];
load_vx = load_pos_d(1);
load_vy = load_pos_d(2);
load_vz = load_pos_d(3);
%% Wind Parameters
v_W = params.wind.speed;
R_W_G = params.wind.R_W_G;
%% 2) Calculate State Derivative for each Vehicle
F_Tether_total = [0;0;0];
Tao_tether_total_P = [0;0;0];
for i = 1:params.num_vehicles
%% 2.1) Get Constants
% Multirotor Constants
g = params.multirotors{i}.g;
m = params.multirotors{i}.m;
Jx = params.multirotors{i}.Jx;
Jy = params.multirotors{i}.Jy;
Jz = params.multirotors{i}.Jz;
M = params.multirotors{i}.M;
% Payload Constants
L = params.multirotors{i}.L;
k = params.multirotors{i}.k;
b = params.multirotors{i}.b;
r_Pi_P = params.multirotors{i}.r_Pi_P;
%% 2.2) Get Vehicle States
x = X((i-1)*12 + 1);
y = X((i-1)*12 + 2);
z = X((i-1)*12 + 3);
u = X((i-1)*12 + 4);
v = X((i-1)*12 + 5);
w = X((i-1)*12 + 6);
phi = X((i-1)*12 + 7);
theta = X((i-1)*12 + 8);
psi = X((i-1)*12 + 9);
p = X((i-1)*12 + 10);
q = X((i-1)*12 + 11);
r = X((i-1)*12 + 12);
%% 2.3) Compute Forces and Torques From Propellers on Multirotor
u0 = (i-1)*params.multirotors{i}.numRotors + 1;
un = i*params.multirotors{i}.numRotors;
F_and_Taos = M * U(u0:un);
F = F_and_Taos(1);
Tao_phi = F_and_Taos(2);
Tao_theta = F_and_Taos(3);
Tao_psi = F_and_Taos(4);
%% 2.3) (1-3) Position Derivative ([x_d; y_d; z_d])
R_V_G = rpy_to_R(phi, theta, psi);
pos_d = R_V_G * [u; v; w];
%% 2.4) Compute Force From Slung Load (this might be more complicated)
% Preliminary Calcs
r_Vi_G = [x;y;z];
r_P_G = [load_x; load_y; load_z];
r_Pi_G = R_P_G * r_Pi_P + r_P_G;
t_Pi_G = r_Pi_G - r_Vi_G;
% tether length and direction calcs
l_i = norm(t_Pi_G,2);
t_hat_Pi_G = t_Pi_G ./ l_i;
t_dot_Pi_G = cross(load_omega, r_Pi_G) + load_pos_d;
l_dot_i = dot(t_dot_Pi_G, t_hat_Pi_G);
% tether force calc (with neg tension check)
F_tether_mag = (k.*(l_i - L) + b.*l_dot_i);
if (params.multirotors{i}.NegTension == 0)
F_tether_mag = max(F_tether_mag,0);
end
% Constrain Max tension for debugging
F_tether_mag = min(F_tether_mag, params.multirotors{i}.MaxTension);
F_Tether_i = F_tether_mag .* -1 .* t_hat_Pi_G;
% Add up tether forces
F_Tether_total = F_Tether_total + F_Tether_i;
% Add up torques as well
F_Tether_i_P = R_P_G' * F_Tether_i;
Tao_tether_i_P = cross(r_Pi_P, F_Tether_i_P);
Tao_tether_total_P = Tao_tether_total_P + Tao_tether_i_P;
%% Drag Force on Quadrotor
V_W_Vi_rel_Vi = R_V_G' * R_W_G * [v_W; 0; 0] - [u; v; w];
F_drag_V = - params.wind.C_bar_d * F * V_W_Vi_rel_Vi;
%% 2.5) (4-6) Velocity Derivative ([u_d; v_d; w_d])
F_thrust_V = [0; 0; -F];
F_gravity_V = R_V_G' * [0; 0; m*g];
F_tether_V = - R_V_G' * F_Tether_i;
F_dist_V = params.multirotors{1}.distSTD .* randn(size(F_gravity_V));
F_net_V = F_thrust_V+ F_gravity_V + F_tether_V + F_drag_V + F_dist_V;
vel_d = cross([p;q;r],[u;v;w]) + F_net_V./m_load;
%% 2.6) (7-9) Attitude Derivative ([phi_d; theta_d; psi_d])
att_d = att_der_matrix(phi, theta) * [p; q; r];
%% 2.7) (10-12) Attitude Rate Derivative ([p_d; q_d; r_d])
Tao_net_V = [Tao_phi; Tao_theta; Tao_psi];
attRate_d = attRate_der(Jx,Jy,Jz,p,q,r,Tao_net_V);
% Collect Forces and Torques and Tether States
F_nets = [F_nets;
F_net_V];
Tao_nets = [Tao_nets;
Tao_net_V];
F_tethers = [F_tethers;
F_tether_V];
F_drags = [F_drags;
F_drag_V];
Tether_states = [Tether_states;
l_i;
l_dot_i];
%% 2.8) Combine and append to xdot vector
xdot = [xdot;
pos_d;
vel_d;
att_d;
attRate_d];
end
%% 3) Calculate State Derivative for the Payload
%% 3.1) (13-15) Slung Load Position (load_pos_d from above)
%% 3.2) (16-18) Slung Load Velocity
% Drag Force on Payload
R_W_P = R_P_G' * R_W_G;
R_P_W
V_W_P_rel_W = [v_W; 0; 0] - [u; v; w]
V_W_P_rel_P = R_V_G' * R_W_G * [v_W; 0; 0] - [u; v; w];
F_drag_V = - params.wind.C_bar_d * F * V_W_Vi_rel_Vi;
V_W_P_rel_P
F_drag_W = f_drag * V_W_P_rel_W / v_W_rm;
F_drag_P = R_P_G' * R_W_G * F_drag_W;
% Calculate Forces
F_gravity_P = R_P_G' * [0; 0; m_load*g];
F_Tether_total_P = R_P_G' * F_Tether_total;
F_drag_P = [0; 0; 0];
F_load_dist_P = params.payload.distSTD .* randn(size(F_gravity_P));
F_net_P = F_gravity_P + F_Tether_total_P + F_load_dist_P;
load_vel = [load_u; load_v; load_w];
load_vel_d = cross(load_omega,load_vel) + F_net_P./m_load;
%% Slung Load Attitude
load_att_d = att_der_matrix(load_phi, load_theta) * load_omega;
%% Slung Load Attitude Rate
Tao_net_P = Tao_tether_total_P;
load_attRate_d = attRate_der(load_Jx,load_Jy,load_Jz,load_p,load_q,load_r,Tao_net_P);
% Collect Forces and Torques
F_nets = [F_nets;
F_net_P];
Tao_nets = [Tao_nets;
Tao_net_P];
F_drags = [F_drags;
F_drag_P];
%% 3.3) Add payload derivatives to vector
xdot = [xdot;
load_pos_d;
load_vel_d;
load_att_d;
load_attRate_d];
end
......@@ -51,6 +51,10 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
load_vy = load_pos_d(2);
load_vz = load_pos_d(3);
%% Wind Parameters
v_W = params.wind.speed;
R_W_G = params.wind.R_W_G;
%% 2) Calculate State Derivative for each Vehicle
F_Tether_total = [0;0;0];
Tao_tether_total_P = [0;0;0];
......@@ -129,15 +133,16 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
Tao_tether_i_P = cross(r_Pi_P, F_Tether_i_P);
Tao_tether_total_P = Tao_tether_total_P + Tao_tether_i_P;
%% Drag Force on Quadrotor
V_W_Vi_rel_Vi = R_V_G' * R_W_G * [v_W; 0; 0] - [u; v; w];
F_drag_V = - params.wind.C_bar_d * F * V_W_Vi_rel_Vi;
%% 2.5) (4-6) Velocity Derivative ([u_d; v_d; w_d])
F_thrust_V = [0; 0; -F];
F_gravity_V = R_V_G' * [0; 0; m*g];
F_tether_V = - R_V_G' * F_Tether_i;
F_drag_V = [0; 0; 0];
F_dist_V = params.multirotors{1}.distSTD .* randn(size(F_gravity_V));
F_net_V = F_thrust_V+ F_gravity_V + F_tether_V + F_dist_V;
F_net_V = F_thrust_V+ F_gravity_V + F_tether_V + F_drag_V + F_dist_V;
vel_d = cross([p;q;r],[u;v;w]) + F_net_V./m_load;
%% 2.6) (7-9) Attitude Derivative ([phi_d; theta_d; psi_d])
......@@ -173,10 +178,24 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
%% 3.1) (13-15) Slung Load Position (load_pos_d from above)
%% 3.2) (16-18) Slung Load Velocity
% Drag Force on Payload
R_W_P = R_P_G' * R_W_G;
R_P_W = R_W_P';
V_W_P_rel_W = [v_W; 0; 0] - R_P_W * [u; v; w];
v_W_rm = norm(V_W_P_rel_W);
Re = reynolds_num(v_W_rm,params);
C_D = params.wind.C_D(Re);
f_drag = (1/8)*C_D*pi* params.wind.load_d^2 *...
v_W_rm^2 * params.wind.rho_f;
F_drag_W = f_drag * V_W_P_rel_W / v_W_rm;
F_drag_P = R_P_G' * R_W_G * F_drag_W;
% Calculate Forces
F_gravity_P = R_P_G' * [0; 0; m_load*g];
F_Tether_total_P = R_P_G' * F_Tether_total;
F_drag_P = [0; 0; 0];
F_load_dist_P = params.payload.distSTD .* randn(size(F_gravity_P));
F_net_P = F_gravity_P + F_Tether_total_P + F_load_dist_P;
......
function [] = plotData(simData,params)
% plotData_quadrotor(simData,params);
plotData_payload(simData,params);
plotData_tethers(simData,params);
% plotData_payload(simData,params);
% plotData_tethers(simData,params);
plotData_drag(simData,params);
end
......
function [] = plotData_drag(simData,params)
%plotData_drag
%% 1) Plot Net Drag Forces on Vehicles
% F_drags = zeros(length(simData.T),params.num_vehicles*3);
F_drag_mags = zeros(length(simData.T),params.num_vehicles);
for i=1:params.num_vehicles
F_drag_mags(:,i) = vecnorm(simData.F_drags(:,i*3-2:i*3)')';
end
figure(401);
legend_entries = cell(1,params.num_vehicles);
plot(simData.T, F_drag_mags(:,1));
legend_entries{1} = "1";
hold on;
for i=2:params.num_vehicles
plot(simData.T, F_drag_mags(:,i));
legend_entries{i} = string(i);
end
hold off;
ylabel('Force (N)');
title('Drag Force Magnitudes');
legend(legend_entries);
movegui(gcf,'west');
movegui(gcf,'onscreen');
end
......@@ -10,7 +10,7 @@
addpath(genpath(pwd))
%% Set Up Parameters
params = setUpParameters_sphere_1_stationary();
params = setUpParameters_sphere_2();
%% Simulate
simData = runSimulation(params);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment