Issuu on Google+

QUADROTOR CONTROL, STABILIZATION & TELEOPERATION WITH KINECT

BUGRA TURAN | PHISIT JORPHOCHAUDOM | SARUNYU HEMHONG

AMIN ATRASH Ph.D.

University of Southern California Viterbi School of Engineering APRIL 2012

1


Table of Contents Table of Contents .............................................................................................................................................. 2 Figure List ........................................................................................................................................................... 3 1. Abstract ....................................................................................................................................................... 4 Introduction ................................................................................................................................................... 5 2. QUADROTOR MODELING ............................................................................................................... 5 Modeling Assumptions ................................................................................................................................. 7 Design Parameters......................................................................................................................................... 7 Rhinoceros Quadrotor CAD Model Drawings ........................................................................................ 8 3. CONTROLLER DESIGN ...................................................................................................................11 Controller Design &Performance .............................................................................................................11 MATLAB Simulink Controller Simulations ............................................................................................14 4. QUADROTOR SIMULATION ..........................................................................................................16 5. QUADROTOR TELEOPERATION ................................................................................................18 Body Tracking ..............................................................................................................................................18 Teleoperation via Kinect ............................................................................................................................19 6. Conclusions ..............................................................................................................................................20 References .........................................................................................................................................................21 APPENDIX I- MATLAB & C++ SOURCE CODES ............................................................................22 Teleoperation Code Explanation...................................................................................................................25

Quadrotor Simulation Video Link

2


Figure List Figure 1 Hector_quadrotor Package Sample Quadrotor Model ................................................................. 5 Figure 2 Hector_quadrotor Package Sample Quadrotor Model ................................................................. 6 Figure 3 Hector_quadrotor Package Sample Quadrotor Model ................................................................. 6 Figure 4 Hector_quadrotor Package Sample Quadrotor Model ................................................................. 7 Figure 5 Custom Built Quadrotor Model Top Perspective ......................................................................... 8 Figure 6 Custom Built Quadrotor Model Cross Top Perspective .............................................................. 9 Figure 7 Custom Built Quadrotor Model Front Perspective ...................................................................... 9 Figure 8 Custom Built Quadrotor Top Perspective ...................................................................................10 Figure 9 Custom Built Quadrotor Model Rendered Perspective .............................................................10 Figure 10 Hovering ..........................................................................................................................................11 Figure 11 Thruster Movement .......................................................................................................................11 Figure 12 Roll Movement ...............................................................................................................................11 Figure 13 Pitch Movement .............................................................................................................................12 Figure 14 Yaw Moment ..................................................................................................................................12 Figure 15 Matlab Simulink Model .................................................................................................................14 Figure 16 Yaw,Pitch and Roll angle Transient Response Characteristics................................................15 Figure 17 Matlab 3D Display Simulation .....................................................................................................15 Figure 18 Outdoor Simulation .......................................................................................................................16 Figure 19 Simulation Nodes and Topics ......................................................................................................17 Figure 20 Body Movement Commands on Rviz with Skeleton_Tracker................................................18

3


1. Abstract This project explores the feasibility of Microsoft Kinect sensor usage for quadrotor teleoperation simulation on gazebo. Quadrotors are used for different purposes because of their simple structure and decent load capacity. When compared with helicopters, quadrotors are easy to stabilize and control. Quadrotors are useful for close area monitoring, bomb searches in difficult areas, search and rescue purposes when they carry camera. One of the main objective in quadrotor design is to increase its payload capacity while maintaining stability. The goal of this project is to provide a new approach for teleoperation in order to test stability of the quadrotor in simulation. Even though teleoperation with keyboard is possible, during simulation using keyboard for teleoperation limits user for value input into controllers. Teleoperation with Kinect provides advantage of using keyboard if any tuning is needed for controllers. Tools such as computer aided design, MATLAB Simulink real-time simulations, MATLAB transient response analysis in 3 Degree of freedom, Robotics Operating System are utilized to address different issues encountered throughout the project development. As final configuration teleoperation of Quadrotor with Microsoft Kinect camera was achieved successfully. Simulation results showed that controllers are well-tuned enough to maintain stability of the quadrotor even for fast direction command changes.

4


Introduction The goal of the quadrotor is to maintain a hover at a user-defined altitude while minimizing lateral drift. Quadrotors are symmetrical vehicles with four equally sized rotors at the end of four equal length rods. Quadrotors make use of multiple rotors allowing for a greater amount of thrust and consequently a greater amount of maneuverability. Also, the quadrotors symmetrical design allows for easier control of the overall stability for the flight. This project is organized as follows, in the second chapter custom quadrotor modeling achieved, in the third chapter controllers were built and tuned for quadrotor, quadrotor simulation was tested in the fourth chapter, finally quadrotor simulation with body tracking objective was realized in the 5th chapter.

2. QUADROTOR MODELING Even though hector_quadrotor simulation package comes with a basic quadrotor model ,so that the quadrotor dimension values are not practical and it was not clear whether external forces were taken into account or not for controller implementation. Thus custom model was needed for controller design, flight simulation and quadrotor teleoperation via kinect. Quadrotor is considered to be built with aluminium which has density of 2.8g/cm^3. ( 0.0000028 kilogram/cubic millimeter) Quadrotor model provided with hector_quadrotor package has following non-realistic dimensions; Total Length = 0.08mm Cumulative Area=0.5mm^2 Mass=0.14 gram Simulation Package Given Quadrotor Models (Units in milimeter)

Figure 1 Hector_quadrotor Package Sample Quadrotor Model

5


Figure 2 Hector_quadrotor Package Sample Quadrotor Model

Figure 3 Hector_quadrotor Package Sample Quadrotor Model

6


Figure 4 Hector_quadrotor Package Sample Quadrotor Model

Modeling Assumptions In order to create practical quadrotor model following assumptions were made, 1)System input is considered as propeller speed, 2)System output is considered as position and attitude, 3)Center of Gravity and body frame origin are assumed to coincide, 4)Structure is assumed to be symmetric so diagonal inertia matrix is used.

Design Parameters Following parameters are used to model our quadrotor dynamics, g = 9.81; [m/s^2] Quadrotor body mass (m) : 0.4794; (kg) Arm length (l) : Planar distance between Center of Gravity and rotor plane, (20,33cm) Rotor height (h) :Height between center of gravity and rotor plane,(4.52cm) Thrust Constant (b) :In order to calculate thrust force with the formula Thrust Force = b* (propeller rotational speed)^2 (3.13e-5) Drag Constant(d) :In order to calculate drag moment with the formula Drag moment= d* (propeller rotational speed)^2 (9e-7)

7


Rotor Inertia (Jr) rotational acceleration Inertia matrix (I) Ixx = 0.0086; [kg.m^2] Iyy = 0.0086; [kg.m^2] Izz = 0.0172; [kg.m^2] gravity vector offset = 0.0

:Inertial counter torque moment is calculated as Jr*propeller :Diagonal matrix with the following values used

Rhinoceros Quadrotor CAD Model Drawings Quadrotor model built different perspective views according to practical values (units in centimeter )

Figure 5 Custom Built Quadrotor Model Top Perspective

8


Figure 6 Custom Built Quadrotor Model Cross Top Perspective

Figure 7 Custom Built Quadrotor Model Front Perspective

9


Figure 8 Custom Built Quadrotor Top Perspective

Figure 9 Custom Built Quadrotor Model Rendered Perspective

10


3. CONTROLLER DESIGN Controller Design &Performance Basic Control Concept of Quadrotor is as follows,

Figure 10 Hovering

In hovering state (Fig 10), the front and the rear propellers rotate counter-clockwise, while the left and the right ones turn clockwise. All the propellers have the same speed.

Figure 11 Thruster Movement

In throttle state (Fig 11), this state is controlled by increasing (or decreasing) all the propeller speeds by the same amount.

Figure 12 Roll Movement

11


Roll state (Fig 12), is controlled by increasing (or decreasing) the left propeller speed and by decreasing (or increasing) the right one. It leads to a torque with respect to the the quadrotor turn

axis which makes

Figure 13 Pitch Movement

.

Pitch state (Fig 13), is very similar to the roll and is controlled by increasing (or decreasing) the rear propeller speed and by decreasing (or increasing) the front one. It leads to a torque with respect to the

axis which makes the quadrotor turn.

Figure 14 Yaw Moment

Yaw state (Fig 14), is controlled by increasing (or decreasing) the front-rear propellers’ speed and by decreasing (or increasing) that of the left-right couple. It leads to a torque with respect to the axis which makes the quadrotor turn. In order to control thrusters, the objective is to find each propeller’s speed. (

PD control was used for altitude and attitude control.

12


13


MATLAB Simulink Controller Simulations

Figure 15 Matlab Simulink Model

Simulink model includes noise and motor dynamics switches, in order to provide robust controller tuning. Simulation desired values are selected as, Roll = 0.2 rad Yaw = 0 rad

Pitch = 0 rad Altitude = 5 m

14


Figure 16 Yaw,Pitch and Roll angle Transient Response Characteristics

Figure 17 Matlab 3D Display Simulation

15


4. QUADROTOR SIMULATION

Figure 18 Outdoor Simulation

Hector_quadrotor_demo package is used for simulation purposes.Simulation package uses sonar and pressure sensor for altitude control, gps_velocity for angular velocity measurement, imu includes accelerometer (linear acceleration measurement ) and gyroscope (rate of turn measurement). Tf is utilized for coordinate transformations cmd_vel is the subscribed topic, that controllers are attached to control velocity of thrusters. Below all the subscribed and published topics are listed. Publications: * /camera/image/compressed/parameter_updates [dynamic_reconfigure/Config] * /gps_velocity [geometry_msgs/Vector3Stamped] * /gazebo/model_states [gazebo_msgs/ModelStates] * /gazebo/parameter_descriptions [dynamic_reconfigure/ConfigDescription] * /raw_imu [sensor_msgs/Imu] * /pressure_height [geometry_msgs/PointStamped] * /tf [tf/tfMessage] * /sonar_height [sensor_msgs/Range] * /gazebo/parameter_updates [dynamic_reconfigure/Config] * /magnetic [geometry_msgs/Vector3Stamped] Subscriptions: 16


* /set_update_rate [unknown type] * /clock [rosgraph_msgs/Clock] * /gazebo/set_model_state [unknown type] * /cmd_vel [unknown type]

Figure 19 Simulation Nodes and Topics

17


5. QUADROTOR TELEOPERATION Body Tracking Openni_kinect stack that contains openni_camera and openni_tracker drivers used, in order to capture body movements to provide movements for quadrotor. Pi_tracker and skeleton_markers packages from Pi Robot repository was used to see how the robot tracks skeleton. Multiple tracking tests were done with changing the distance between body and kinect. It is assured that tracking of skeleton movements is guaranteed under 3.5 feet range. Rviz 3D visualization environment was used to visualize real time body moevements. Rviz screen captures for stop, up, left rotate, right rotate, left turn, right turn, down commands are depicted below.

Figure 20 Body Movement Commands on Rviz with Skeleton_Tracker

In order to define arm movements to control quadrotor movements, two different fixed points in the skeleton was tested. Both neck and torso fixed points accuracy was analyzed. After running several simulations it is realized that using neck as fixed point gives not only faster response but also more accurate transition between states. During simulations it is concluded that, when torso 18


is defined as fixed point, kinect does not track fast transitions between arm movements. As an example for given rise up command (both arms are pointing upwards ), if the arm is immediately moved to left turn (left arm makes straight angle with torso) , quadrotor hesitates between stop and left turn.

Teleoperation via Kinect As mentioned above, Kinect tracked body movements and published to ROS message system by openni_tracker. However, information about body movements was defined by translation vector. Translation vector define how particular object move in 3d space. Translation vectors are in 3d space. Thus, arm movements can be tracked using these translation vectors. In order to successfully perform teleoperation, simple set of arm movements have been defined. Basically, translation vector value in y-axis increases when arm moves upward and vice versa. These movements hold true in x-axis(horizontal movement) and z-axis(distance from body to kinect) as well. After arm movements were tracked and translated into translation vectors, simple set of commands were defined according to those movements. In this project, arm movements were translated into commands as following.

Left Arm

Right Arm

Command

Move upward

Move upward

Increase height

Stretch to side

Stretch to side

Decrease height

No movement/Move position

to

Stretch to side No movement/Move position

No movement/Move position to

Move upward No movement/Move position

stand Stretch to side

to

Go forward to

stand Move upward

stand Go backward Rotate clockwise

counter-

No movement/Move position

to

stand Rotate clockwise

stand No movement/Move position

to

stand Stop

19


6. Conclusions Quadrotor with practical dimensions was created in Rhinoceros CAD software platform. Area and thickness was calculated using CAD software. Quadrotor main frame assumed to be built from aluminium and density of quadrotor was considered as 2.8g/cm^3. As a result quadrotor mass is calculated to be 0.4794kg. When compared to the model provided by hector_quadrotor package, this model is almost 65% larger in size and 40% heavier in weight. Controllers for 3 degree of freedom built according to the model created. Even though first attempt was to implement PID controllers, so that desired command tracking obtained without integral gain, controllers selected as PD controllers. MATLAB Simulink simulation with transient response analysis in 3 degree of freedom considered for a robust PD controller in order to ensure stability for the noisy flight conditions. Proportional and derivative gains are tuned for optimum response and settling time. Microsoft Kinect was used for teleoperation of the quadrotor. Different fixed points in skeleton were considered to obtain fastest and most accurate response. Movements with respect to neck is found to be the most accurate teleoperation method for quadrotor. Quadrotor stabilization, control and teleleoperation with Kinect goals are successfully achieved by the usage of hector_quadrotor_demo simulation package. Kinect teleoperation was used both indoor and outdoor simulations, with the PD controller implementations .Kinect teleoperation was found to be useful in order to evaluate quadrotor modeling accuracy and controller performance. Kinect teleoperation gives user more flexibility in terms of movements. Keyboard, x-box, ps3 teleoperation is possible for quadrotor simulation. If controller tuning is the objective, usage of these methods will limit user to run simulation then tune the parameters. With kinect teleoperation user is still able to run the simulation ,control movements while using keyboard to feed input for tuning controller parameters. Video link is provided showing complete quadrotor simulation with kinect teleoperation.

Quadrotor Simulation Video Link

20


References http://www.grc.nasa.gov/WWW/K-12/airplane/short.html (general) https://dspace.ist.utl.pt/bitstream/2295/574042/1/Tese_de_Mestrado.pdf http://www.wpi.edu/Pubs/E-project/Available/E-project-031411140744/unrestricted/MQP_Rotorheads_Final_Report.pdf [4] http://www.ros.org/wiki/pi-robot-ros-pkg [5] http://www.ros.org/wiki/kinect [6] http://ros.org/wiki/openni_tracker [1] [2] [3]

21


APPENDIX I- MATLAB & C++ SOURCE CODES Source Code #include <iostream> #include <geometry_msgs/Twist.h> #include <ros/ros.h> #include <geometry_msgs/TransformStamped.h> #include <tf/transform_listener.h> #include "std_msgs/String.h" /* * numState: * Convert translation vector from Transform Stamp object * into position of arm. * */ int numState(geometry_msgs::TransformStamped &msg1){ float positionY = msg1.transform.translation.y; int num = 0; if (positionY < -0.4){ num = 1; } else if(positionY>-0.3 && positionY<0.3){ num = 2; } else if(positionY>0.4){ num = 3; } return num; } /* * checkState * Convert position of arm into machine state. */ int checkState(int right, int left){ int ret = 0; if(right==3 && left==3 ){ //up ret = 1; } else if(right==2 && left==2 ){ //down ret = 2; 22


} else if(right==1 && left==2 ){ //forward ret = 3; } else if(right==2 && left==1 ){ //back ret = 4; } else if(right==3 && left==1 ){ //rotate counter-clock ret = 5; } else if(right==1 && left==3 ){ //rotate clock |_ O (down) ret = 6; } else if(right==1 && left==1 ){ //stop stand ret = 7; } else { ret = 8; } return ret; }

int main(int argc, char** argv) { //init the ROS node ros::init(argc, argv, "robot_driver"); // ROS node ros::NodeHandle nh; // Command publisher ros::Publisher cmd_vel_pub_; // Listener to listen to kinect publisher tf::TransformListener listener_; // Stamped transform tf::StampedTransform right_hand; tf::StampedTransform right_elbow; tf::StampedTransform left_hand; tf::StampedTransform left_elbow; // Message received from listener geometry_msgs::TransformStamped msg_right_hand; geometry_msgs::TransformStamped msg_left_hand; geometry_msgs::TransformStamped msg_right_elbow; geometry_msgs::TransformStamped msg_left_elbow; // Advertise to command publisher which connect to Gazebo cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); bool check = true; geometry_msgs::Twist base_cmd; base_cmd.linear.y = base_cmd.angular.z = 0; 23


while(check) { try { // Listen to Kinect and retrieve transform object of // right hand of user 1 relative to neck of user 1 listener_.lookupTransform("neck_1", "right_hand_1", ros::Time(0), right_hand); // Listen to Kinect and retrieve transform object of // left hand of user 1 relative to neck of user 1 listener_.lookupTransform("neck_1", "left_hand_1", ros::Time(0), left_hand); // Convert transform stamped object to ROS message tf::transformStampedTFToMsg(right_hand, msg_right_hand); tf::transformStampedTFToMsg(left_hand, msg_left_hand); // Find position of right hand and left hand. int state_right = numState(msg_right_hand); int state_left = numState(msg_left_hand); // Translate into machine's state int state = checkState(state_right, state_left); // Output to console ROS_INFO("State is %d", state); // Select case for state machine. switch(state){ case 1: // increase height from the ground base_cmd.linear.z = 5.0; break; case 2: // decrease height base_cmd.linear.z = -5.0; break; case 3: // Move to positive X-axis base_cmd.linear.x = 5.0; break; case 4: // Move to negative X-axis base_cmd.linear.x = -5.0; break; case 5: // Rotate counter-clockwise base_cmd.angular.z = 5.0; break; case 6: // Rotate clockwise base_cmd.angular.z = -5.0; break; case 7: // Stop base_cmd.linear.x = 0.0; 24


}

base_cmd.linear.y = 0.0; base_cmd.linear.z = 0.0; base_cmd.angular.x = 0.0; base_cmd.angular.y = 0.0; base_cmd.angular.z = 0.0; break; case 8: // Stop base_cmd.linear.x = 0.0; base_cmd.linear.y = 0.0; base_cmd.linear.z = 0.0; base_cmd.angular.x = 0.0; base_cmd.angular.y = 0.0; base_cmd.angular.z = 0.0; break;

// publish command to topic "cmd_vel" cmd_vel_pub_.publish(base_cmd);

}

} catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); }

}

Teleoperation Code Explanation

// ROS node

ros::NodeHandle nh;

Node handle object handles communication between program and ROS. // Command publisher

ros::Publisher cmd_vel_pub_; ROS publisher publishes data to specified topic. In this program, topic named cmd_vel is being used.

// Listener to listen to kinect publisher tf::TransformListener listener_;

Topic listener object listens to ROS. In our implementation, this object is being used to listen to Kinect commands. // Stamped transform tf::StampedTransform right_hand; tf::StampedTransform right_elbow; 25


tf::StampedTransform left_hand; tf::StampedTransform left_elbow;

Stamped transform object is a message format received from transform listener. // Message received from listener

geometry_msgs::TransformStamped msg_right_hand; geometry_msgs::TransformStamped msg_left_hand; geometry_msgs::TransformStamped msg_right_elbow; geometry_msgs::TransformStamped msg_left_elbow;

geometry_msgs is a data structure which is being used to hold information from Kinect. // Advertise to command publisher which connect to Gazebo

cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);

Tell ROS that cmd_vel_pub is publishing data to topic named â&#x20AC;&#x153;cmd_velâ&#x20AC;?. geometry_msgs::Twist base_cmd; base_cmd.linear.y = base_cmd.angular.z = 0;

base_cmd is a geometry_msgs. This variable is being used to send command to cmd_vel_pub. The second line initialize velocity to zero. // Listen to Kinect and retrieve transform object of // right hand of user 1 relative to neck of user 1 listener_.lookupTransform("neck_1", "right_hand_1", ros::Time(0), right_hand);

// Listen to Kinect and retrieve transform object of // left hand of user 1 relative to neck of user 1 listener_.lookupTransform("neck_1", "left_hand_1", ros::Time(0), left_hand);

Listener look up for Kinect's command and use stamped transform object to contain data. // Convert transform stamped object to ROS message tf::transformStampedTFToMsg(right_hand, msg_right_hand); tf::transformStampedTFToMsg(left_hand, msg_left_hand);

Convert stamped transform object into ROS message. int numState(geometry_msgs::TransformStamped &msg1){ float positionY = msg1.transform.translation.y; int num = 0; if (positionY < -0.4){ num = 1; 26


} else if(positionY>-0.3 && positionY<0.3){ num = 2; } else if(positionY>0.4){ num = 3; } return num; }

According to empirical data, translation vector from transform stamped message can be translated into hand position. int checkState(int right, int left){ int ret = 0; if(right==3 && left==3 ){ //up ret = 1; } else if(right==2 && left==2 ){ //down ret = 2; } else if(right==1 && left==2 ){ //forward ret = 3; } else if(right==2 && left==1 ){ //back ret = 4; } else if(right==3 && left==1 ){ //rotate counter-clock ret = 5; } else if(right==1 && left==3 ){ //rotate clock |_ O (down) ret = 6; } else if(right==1 && left==1 ){ //stop stand ret = 7; } else { ret = 8; } return ret; }

This function translates hands' position into state. Each state represents different movement for robot. // Output to console

ROS_INFO("State is %d", state);

ROS_INFO prints desired text to console. 27


// Select case for state machine.

switch(state){ case 1: // increase height from the ground base_cmd.linear.z = 5.0; break; case 2: // decrease height base_cmd.linear.z = -5.0; break; case 3: // Move to positive X-axis base_cmd.linear.x = 5.0; break; case 4: // Move to negative X-axis base_cmd.linear.x = -5.0; break; case 5: // Rotate counter-clockwise base_cmd.angular.z = 5.0; break; case 6: // Rotate clockwise base_cmd.angular.z = -5.0; break; case 7: // Stop base_cmd.linear.x = 0.0; base_cmd.linear.y = 0.0; base_cmd.linear.z = 0.0; base_cmd.angular.x = 0.0; base_cmd.angular.y = 0.0; base_cmd.angular.z = 0.0; break; case 8: // Stop base_cmd.linear.x = 0.0; base_cmd.linear.y = 0.0; base_cmd.linear.z = 0.0; base_cmd.angular.x = 0.0; base_cmd.angular.y = 0.0; base_cmd.angular.z = 0.0; break; }

This switch-case statement checks for state and assign a command to base_cmd. // publish command to topic "cmd_vel" cmd_vel_pub_.publish(base_cmd);

Publish command to cmd_vel_pub. As mentioned above, cmd_vel_pub_ is being advertised to cmd_vel topic. Topic: 28


In this program, topic named cmd_vel has been used to fulfill project implementation. cmd_vel is the topic which control velocity of the quadrotor. MATLAB System_Dynamics.m In the matlab file system_dynamics forces, states, system inputs and output defined as follows, x1 = in(1); x position x2 = in(2); xdot velocity x3 = in(3); y position x4 = in(4); ydot velocity x5 = in(5); z position x6 = in(6); zdot velocity x7 = in(7); phi roll angle x8 = in(8); body x angular velocity x9 = in(9); theta pitch angle x10 = in(10); body y angular velocity x11 = in(11); psi yaw angle x12 = in(12); body z angular velocity system inputs Om_1 = in(13); Om_ stands for propeller rotational speeds Om_2 = in(14); Om_3 = in(15); Om_4 = in(16); U1 = b*(Om_1^2 + Om_2^2 + Om_3^2 + Om_4^2); U2 = b*(-Om_2^2 + Om_4^2); U3 = b*(Om_1^2 - Om_3^2); U4 = d*(-Om_1^2 + Om_2^2 - Om_3^2 + Om_4^2); omega = - Om_1 + Om_2 - Om_3 + Om_4; - first order differential equations -----------------------------xdot = x2; xddot = -1/m * (cos(x7)*sin(x9)*cos(x11) + sin(x7)*sin(x11)) * U1; ydot = x4; yddot = -1/m * (cos(x7)*sin(x9)*sin(x11) - sin(x7)*cos(x11)) * U1; zdot = x6; zddot = g - (1/m)*cos(x7)*cos(x9) * U1; phidot = x8+sin(x7)*sin(x9)/cos(x9)*x10+cos(x7)*sin(x9)/cos(x9)*x12; omegaxdot = x10*x12*(Iyy-Izz)/Ixx + Jr/Ixx*x10*omega + L/Ixx * U2; thetadot = cos(x7)*x10+sin(x7)*x12; omegaydot = x8*x12*(Izz-Ixx)/Iyy - Jr/Iyy*x8*omega + L/Iyy * U3 - gOffset*m*g*cos(x9); psidot = sin(x7)/cos(x9)*x10+cos(x7)/cos(x9)*x12; omegazdot = x8*x10*(Ixx-Iyy)/Izz + U4/Izz; function output - state variable derivatives -------------------out(1) = xdot; xdot velocity out(2) = xddot; xddot acceleration out(3) = ydot; ydot velocity out(4) = yddot; yddot acceleration out(5) = zdot; zdot velocity 29


out(6) = zddot; out(7) = phidot; out(8) = omegaxdot; out(9) = thetadot; out(10) = omegaydot; out(11) = psidot; out(12) = omegazdot;

zddot acceleration phidot roll velocity phiddot acceleration thetadot roll velocity thetaddot acceleration psidot roll velocity psiddot acceleration

30


Quadrotor Control, Stabilization & Teleoperation With Microsoft Kinect