ECPS 208代写、Java/Python程序代写

日期：2023-05-21 04:51

ECPS 208: Control Systems for Cyber Physical Systems

Homework Assignment Number 4

Higher Dimensional Quadcopter - Trajectory Control

Spring 2023

Consider the case of a 2D quadcopter. We would like to control the quadcopter for a variety of

2D trajectories (in the y-z directions): straight line trajectory, sine wave trajectory, step trajectory.

The files for this section are contained in the folder: “hw4 2d.zip”.

Figure 1: 2D Quadcopter Model.

1 Derivations

Our 2D quadcopter is depicted in Figure 1. The global, or inertia frame, is shown as {a2, a3} and

the body frame of the quadcopter is depicted as {b2, b3}. In the 2D motion, the roll angle ϕ is

shown and blue, and we can model the rotation matrix between the {A} and {B} frames as:

ARB =



cos(ϕ) − sin(ϕ)

sin(ϕ) − cos(ϕ)



(1)

The angular velocity is modeled as:

AωB = ϕ˙ (2)

We will make some simplifying assumptions in our model of the inputs. We assume only two

motors that can each apply a thrust F1, F2, located a distance L away from the quadcopters center

of mass. Our quadcopter has two inputs, u1 (thrust) and u2 (moment):

u1 = F1 + F2 (3)

u2 = L(F1 − F2) (4)

1

The state space of our system is defined by three states: y, z, and ϕ. The equations of motion

for the state variables can be derived as:

my¨ = −u1 sin(ϕ) (5)

mz¨ = −mg + u1 cos(ϕ) (6)

Ixxϕ¨ = u2 (7)

where Ixx is the moment of inertia about the x axis.

1.1 Linearization

Linearize the above state space equations about a hovering equilibrium. Assume as this hover that

ϕ = 0 and the thrust force balances out the gravity.

2 PD Control

We will design a PD controller and derive the equations for our inputs in the controller function.

As we have three states y, z, and ϕ, we will need a set of gains (Kp, Kd) for each state. We form

the following three differential equations that aim to converge the error of each state to zero:

(¨ydes − y¨) + Kpy(ydes − y) + Kdy( ˙ydes − y˙) = 0 (8)

(¨zdes − z¨) + Kpz(zdes − z) + Kdz( ˙zdes − z˙) = 0 (9)

(ϕ¨

des − ϕ¨) + Kpphi(ϕdes − ϕ) + Kdphi(ϕ˙

des − ϕ˙) = 0 (10)

The inputs u1 and u2 are derived as follows:

u1 = m(g + ¨z + Kpz(zdes − z) + Kdz( ˙zdes − z˙)) (11)

u2 = Ixx(ϕ¨ + Kpphi(ϕdes − ϕ) + Kdphi(ϕ˙

des − ϕ˙)) (12)

ϕ = −(¨ydes + Kpy(ydes − y) + Kdy( ˙ydes − y˙))/g (13)

ϕ˙ = (KpyKdy(ydes − y) + (K2

dy − Kpy) ∗ ( ˙ydes − y˙))/g (14)

2.1 PD Controller

Update the file “pd controller 2d.m” following the above equations. Set the controller gains to any

values you feel appropriate as they will be tuned further in the following sections.

function [ u1, u2 ] = pd_controller_2d(~, state, des_state, params)

%CONTROLLER Controller for the planar quadrotor

%

% state: The current state of the robot with the following fields:

% state.pos = [y; z], state.vel = [y_dot; z_dot], state.rot = [phi],

% state.omega = [phi_dot]

%

% des_state: The desired states are:

2

% des_state.pos = [y; z], des_state.vel = [y_dot; z_dot], des_state.acc =

% [y_ddot; z_ddot]

%

% params: robot parameters

% Using these current and desired states, you have to compute the desired

% controls

kdy = ...;

kpy = ...;

kdz = ...;

kpz = ...;

kdphi = ...;

kpphi = ...;

phic = ...;

phic_dot = ...;

u1=...;

u2 = ...;

end

2.2 Trajectory Control

Use the following script “runsim.m” and tune your controller gains to have the quadcopter reach

its goal for each type of trajectory: (a) Straight Line Trajectory, (b) Sine Wave Trajectory, (c) Step

Trajectory. Bonus: feel free to try the Diamond Trajectory as well.

clear;

close all;

addpath(’utils’);

addpath(’trajectories’);

controlhandle = @pd_controller_2d;

% Choose which trajectory you want to test with:

% @traj_line, @traj_sine, @traj_step , @traj_diamond

trajhandle = @traj_line;

%trajhandle = @traj_sine;

%trajhandle = @traj_step;

%trajhandle = @traj_diamond;

[t, state] = simulation_2d(controlhandle, trajhandle);

3

3 Controller Robustness

In this section, we investigate the robustness of controller design with regards to uncertainties in

both modeling and sensing. Recall the general form of a closed loop control system in Figure 2.

Figure 2: General form of a closed loop control system.

3.1 Plant Disturbances

In our system, an example of a plant disturbance is an incorrect or incomplete model of the forces

acting on the quadcopter (thrust, air resistance, friction, etc.) or on the model parameters we have

measured about the quadcopter (e.g., mass, length).

Our noise model will simply add a wind disturbance to the y and z directions following:

y¨ = −u1 sin(ϕ)/m + λddy (15)

z¨ = −g + u1 cos(ϕ)/m + λddz (16)

where dy and dz are the added noise values due to random wind: d ∼ N (µ, σ2

) with µ = 0 and

σ = 1; and λd is our tunable scaling of disturbance noise.

Run the following script “runsim plant disturbances.m” with varying levels of noise (λd =

1, 2, 5, 10, 20) and trajectories (line, sine, step) and comment on the results.

clear;

close all;

addpath(’utils’);

addpath(’trajectories’);

controlhandle = @pd_controller_2d;

% Choose which trajectory you want to test with:

4

% @traj_line, @traj_sine, @traj_step , @traj_diamond

%trajhandle = @traj_line;

%trajhandle = @traj_sine;

trajhandle = @traj_step;

%trajhandle = @traj_diamond;

%Define the level of plant noise: lambda_d

%Our noise will be modeled as wind which affects y’’ and z’’.

noise_level = ...; %1, 2, 5, 10, 20

[t, state] = simulation_2d_plant_disturbance(controlhandle, trajhandle, noise_level);

3.2 Sensor Measurement Noise

Noise in sensor measurements is very common, and one benefit of control is accounting for variabilities in sensing noise. In our closed-loop controller, we assume access to a sensor that measures

the position and the velocity of the quadcopter in the direction. We will now study what happens

when the sensing in the system is not perfect. We introduce noise into the y and z position sensors

as follows:

ymeas = y + λnny (17)

zmeas = z + λnnz (18)

where ymeas and zmeas are the sensor measurements the controller has access to; ny and nz are

the added noise values due to sensing error: n ∼ N (µ, σ2

) with µ = 0 and σ = 1; and λn is our

tunable scaling of the sensor noise level.

Run the following script “runsim sensor noise.m” with varying levels of noise (λn = 0.01, .05, 0.1, 0.5)

and trajectories (line, sine, step) and comment on the results.

clear;

close all;

addpath(’utils’);

addpath(’trajectories’);

controlhandle = @pd_controller_2d;

% Choose which trajectory you want to test with:

% @traj_line, @traj_sine, @traj_step , @traj_diamond

%trajhandle = @traj_line;

%trajhandle = @traj_sine;

trajhandle = @traj_step;

%trajhandle = @traj_diamond;

%Define the level of sensor noise: lambda_n

%Our noise will be modeled as wind which affects y’’ and z’’.

noise_level = ...; % 0.01, .05, 0.1, 0.5

5

[t, state] = simulation_2d_sensor_noise(controlhandle, trajhandle, noise_level);

4 Bonus: 3D Trajectory Control

There are no graded questions within this section. We encourage you to explore the code at your

own interest. The files for this section are contained in the folder: “hw4 3d.zip”. Make sure to

remove the utils folder from your path to avoid errors if you have recently run the code for the 2D

case.

Consider the case of a 3D quadcopter. The code for the dynamics and controller has been

provided for you in the files within these folders. We would like to control the quadcopter for a

variety of 3D trajectories (in the x-y-z directions): Line, Helix, Waypoint. Feel free to experiment

with the code at your own interest. The waypoint trajectory is easily definable to measure your

own custom trajectory path.

6