Home

fulltext

1. References 1 Yorihisa Yamamoto NXTway GS Self Balancing Two Wheeled Robot Con troller Design http www mathworks com matlabcentral fileexchange 19147 2 Takashi Chikamasa Embedded Coder Robot NXT http www mathworks com matlabcentral fileexchange 13399 3 MATLAB for LEGO MINDSTORMS Robots http www mathworks com academia lego mindstorms nxt software legomindstorms matlab html gor UNtp Avdelning Institution Datum 4 J D Division Department Date a Sa 4 7 Division of Automatic Control AN Department of Electrical Engineering egrets ty Na Ska p cs Spr k Rapporttyp ISBN Language Report category a Svenska Swedish Licentiatavhandling ISRN X Engelska English Examensarbete C uppsats A Serietitel och serienummer ISSN uppsats A z Title of series numbering 1400 3902 X Ovrig rapport URL f r elektronisk version LiTH ISY R 3006 http www control isy liu se Titel Lego Segway Project Report Title F rfattare Patrik Axelsson Ylva Jung Author Sammanfattning Abstract This project was a part of the course Applied Control and Sensor Fusion http www control isy liu se student graduate AppliedControl index html during summer and fall 2010 The goal of the course was to be a practical study of imple mentation issues not always encountered in the life of a PhD student A segwa
2. 3 Confirm with the orange button 4 Go back with the dark gray button The program that is downloaded in Section 7 1 is started according to 1 Press the orange button to start the robot 2 Use the triangular and the orange buttons to choose My Files Software files nztway_ app 3 Choose the alternative called run and confirm with the orange button 4 The main menu for the NXT program is now visible The display shows how to continue 5 The robot must be held still in an upright position when the start button right triangular button is pressed Release the robot when a beep sounds 7 USER MANUAL 10 NXTway GS Controller Model based on Rate Monotonic Scheduling This model consists of four parts Device Inputs Device Outputs Task Scheduler and Application Task Subsystem Disclaimer LEGO R is a trademark of the LEGO Group of companies which does not sponsor authorize or endorse this demo LEGO R and Mindstorms R are registered trademarks of The LEGO Group int32 inte aa Go la ty theta mI E ad task_init_fe pwm Revolution Sensor Interface Fe ESA z Servo Motor Interfacet Port C i asks itt ee Port G Pron task _init Init ora Pese mana ai int32 task _ts1 0 004 sec ints ree Ca task_ts2 0 02 sec ee Py task ts2 fc a theta mr task_ts3 0 1 sec task_ts2 pwm_r Revolution Sensor Interfacet Servo Motor Interface Port B ExpFenCalls Schedul
3. the measured angular velocity as y 8 0 0 0 1 1 z 13 It is a bit more tricky to find a relationship between the measured motor angles and 5 OBSERVER 5 the states A glance at the existing solution gave us 1 1 d z 8i H 0 2 Om yY H Om r t wv Om Om r t yp Om t Yp 14 where 6 and 6 are the angles of the left and right wheel 0m and m r are the angles A of the left and right motor which are measured and 06m 4 Om m r The second measurement equation can be written y2 O0m 1 1 0 0 O z 15 and the complete measurement equation with measurement noise added is at last obtained as 0 0 0 1 1 100 j tte Crte 16 Simple calculations show that the model is observable i e the observability matrix O has full rank The stationary KF can be written as amp A KC B K C 17a AP PA P R OP GQG7 0 17b K PTR 17c where R E ee andQ E ww We have also assumed that the cross correla tion between the process noise w and the measurement noise e is zero The MATLAB function lqe has been used to calculate the observer gain K The covariance matrices for the process noise and measurement noise are chosen as Q amp D 18a 0 01 0 i 0 sn 18b 5 2 Implementation The observer has to be discretised before it can be implemented in the Simulink dia gram The discretisation is made with zero order hold that is Lei Aaty Bau
4. 1 03 16 1 Introduction In this project a LEGO segway robot has been built as a part of the course Applied Control and Sensor Fusion http waw control isy liu se student graduate AppliedControl index html The goal of this project course is to be a practical study of implementation issues not always encountered in the life of a PhD student A short presentation of the segway can be found in Section 2 The modeling is described in Section 3 and the controller in Section 4 The initial plan for the project was to construct and implement an H controller and if the project time allowed it construct and implement an observer We started out on an H controller but since we did not like the way of implementing an observer by integrating a sensor measurement we soon switched to observer construction instead The observer is presented in Section 5 and works well on simulated data but when trying it on the real segway it did not work at all To investigate why we tried to log the measured data by connecting the segway to the PC using Bluetooth and a USB cable but we did not succeed see Section 6 In Section 7 a short user manual can be found describing how to use the exist ing programs for e g small demonstrations and how to develop and compile new programs If there had been more time we had most likely tried to design and implement an EKF and or proceed to design and implement an H controller 2 Segway The segway used is based
5. Technical report from Automatic Control at Link pings universitet Lego Segway Project Report Patrik Axelsson Ylva Jung Division of Automatic Control E mail axelsson isy liu se ylvju isy liu se 16th March 2011 Report no LiTH ISY R 3006 Address Department of Electrical Engineering Link pings universitet SE 581 83 Link ping Sweden WWW http www control isy liu se AUTOMATIC CONTROL REGLERTEKNIK LINKOPINGS UNIVERSITET Technical reports from the Automatic Control group in Link ping are available from http www control isy liu se publications Abstract This project was a part of the course Applied Control and Sensor Fusion http www control isy liu se student graduate AppliedControl index html during summer and fall 2010 The goal of the course was to be a practical study of implementation issues not always encountered in the life of a PhD student A segway was constructed using a LEGO Mindstorms NXT kit and a gyro and the goal was to construct a self balancing segway To do this the motor angles and the gyro measurements were available and a working Simulink program The main focus in this project has been to construct an observer The segway can be used for demos in basic control courses and a manual can be found at the end of the report Keywords segway manual observer lego mindstorms Lego Segway Project Report Patrik Axelsson axelsson isy liu se Ylva Jung ylvju isy liu se 201
6. built in the same way as the segway the models were developed for these are judged to be valid in this project Figure 1 shows a simplified two wheeled inverted pendulum from the side and from the top with the coordinates used in the report These are 0 average angle of left and right wheel 0 and 6 yw body pitch angle og body yaw angle and some parameters are presented in Table 1 for the numerical values see 1 4 CONTROLLER 3 Lagrange equation leads to a description of the motion 2m M R Ww 2n7Jm 6 MLReosy 2n Im MLRY siny Fo 1 ML Reosy 2n Jm ML Jy 20 Im MgLsiny ML sinwcosy Fy 2 2 mW H Jo ua Jw 7 Jm ML sin y 6 2ML pdsinycosy Fy 3 These equations are nonlinear but can be linearised around a point of equilibrium in this case the upstanding position of the segway y gt 0 gt siny gt y cosa gt 1 and higher order terms are neglected Equations 1 3 can then be approximated as 2m M R Ww 2n Im MLR 2n Jm b Fo 4 MLR 20 Jm 6 ML Jy 2n Im b MgLy Fy 5 1 w i sw bleh pa Jw 07 JIm Fo 6 where Fo y are the forces in the 0 Y directions respectively The connection between 0 and w and their derivatives is described in Equations 4 and 5 and in Equation 6 So with the state vectors m 6 p 6 m e 7 and the expressions for Fo y as in 1 the moti
7. d in this project 5 Observer The observer that was implemented in 1 integrated the measured angular velocity from the gyro One thing that you learn in a basic course in signal processing is that the measured signal from many sensors are noisy and influenced with drift That is a constant sensor error will grow without limits when integrating the signal Integrating this signal directly will therefore be a bad decision Instead we developed a model based observer to cope with this problem The segway is a nonlinear system that means a nonlinear observer should be used e g Extended Kalman Filter EKF Since we used a linearised model of the segway in upstanding position as described in Section 3 a stationary Kalman Filter KF was used The system can be described by a continuous state space model according to 8 ay Avy Bu 11 where z 0 wo wb A and B are system matrices and u is the voltage to the motors see Section 3 and 1 for more details 5 1 Design We augmented this model with one more state in order to take the drift of the gyro into consideration The new model where we also have included a noise model can be written A 0 B B 0 a sye 6 e o tye 12 S Sa m A B G where x 0 p 6 w 5 is the augmented state vector and w wi wa is the noise vector The measured signals are the angular velocity of the body from the gyro and the motor position of the two motors First we model
8. er ire Pessa Port B Priority Priority 0 n32 lt J nxtway_app inte come fey Gis sonar Ultrasonic Sensor Interface Bluetooth Tx Interface Port S2 Priority 1 Peia 192 uin i ro m Sound Tone Interface Gyro Sensor Interface Priority 1 Port S4 ED yems x Generate code from task subsystem using RTW EC bluetooth _rx Bluetooth Rx Interface Priority 1 Goe pl li 4 Generate code and build the generated code battery Battery Voltage Interface Download NXT enhanced firmware Download SRAM Priority 1 System Clock Interface Priority 1 Figure 5 The main window of the file nrtway __gs__controller mdl The controller is implemented in the subsystem nztway_app 7 3 Develop new Programs This section describes how you can develop your own program in ECRobot NXT It is best to start with natway_gs_controller mdl since all the necessary settings are correct in that file Save the file as something new e g my_terminator and you are ready to develop Start by renaming the subsystem nztway_app to a name of your choice e g terminator Then right click on the button Generate code and build the generated code choose Annotation Properties Finally change the name of the system you want to build in the ClickFcn text editor In other words change from nxtbuild nxtway_app build to nxtbuild terminator build You are no
9. k 19a Yk Cax 19b where A e4 20a Te a i Ba I e Bdt 20b 0 C 20c The sample time T is 4 ms 6 DATA LOGGING 6 The measured gyro data are in degrees s but they are given with an offset An estimate of the offset is obtained if the measured data is averaged over several readings while the segway is standing still This is done during the initialisation task in the Simulink diagram The offset is then subtracted from the measurement before it enters the observer A transformation from degrees s to rad s also has to be done both for the gyro data after the offset is removed and for the measured motor angles 5 3 Result The linearised model was simulated with our observer Figure 3 shows the estimated 0 w 6 and a Note that the estimation starts after 1s because the first second is used to perform the initialisation task We can see that the estimated states follow the true states well There are some differences in the beginning of the second state but it is less than 5 which we thought would be sufficient These differencies probably stem from the peak in the fifth state 6 causing a discrepancy between the true and estimated w thus leading to an error in the y state We have also compared our estimates with the estimates from the existing observer in 1 and they are more or less the same The state 6 that describes the drift in the gyro is shown in Figure 4 The controller is obviously able to hold the seg
10. on LEGO Mindstorms NXT and assembled according to 1 All parts are standard LEGO Mindstorms components except for a single direction gyro sensor from HiTechnic The segway is an implementation of an inverted pendulum and is a nonlinear system The nonlinear model and the linearised model used in this project are further presented in Section 3 Inputs and outputs The actuators of the segway are two DC motors one connected to the left wheel and one to the right To control these actuators the controlled input is e voltage to the motors recalculated to two PWM signals to the left and right DC motor Some of the segway state variables are measured and the outputs from the segway are e the DC motor angles 6m and Om 3 MODEL 2 gt x x Xn x x Figure 1 Side view and plane view of the two wheeled inverted pendulum modeled 1 Table 1 Physical parameters used in the model g gravitational acceleration Jm DC motor moment of inertia Jw wheel moment of inertia Js body yaw moment of inertia Jy body pitch moment of inertia L distance of the mass center from the wheel axis m wheel weight M body weight n gear ratio R wheel radius W body width e the body pitch angular velocity w from the gyro The other states including the body pitch angle 7 have to be calculated and therefore an observer will be used in this project 3 Model The model used is derived by 1 Since the segway has been
11. on equations can be rewritten and divided into two separate state space models 1 Ax Sle B u 8 2 Aox2 Tr Bou 9 where u v vr are the left and right motor voltages In the model in Equa tion 8 with the equations handling the upright position no turning possibility needed v vr so therefore these are simplified to the one input signal u the voltage to the motors The angles 0 and 4 are in radians and the angular velocities 0 and a are in radians s Since this project has focused on the control of 0 and w only the model described in 8 has been considered The nonlinear state equations are linearised around the upstanding position and are thus only valid around that point 4 Controller The controller implemented in 1 is a modified linear quadratic controller with a feedback gain and an integral gain to help control the position of the segway The weight matrices are 1 10 0 Qiar gt Rigr 0 T 108 10 SoooRr oooO ooroo onoo Loooo 4 10 with Ziar 0 Y 6 op f 0 Bret So the Qigr 2 2 element is the weight for w and the Qigr 5 5 element is working on an added state the integrated difference 5 OBSERVER 4 Figure 2 An overview of the modified LQ controller 1 Cg denotes the con version from x to 0 between the angle 0 and the reference 6 cr see Figure 2 for the Simulink block diagram of the controller The controller has not been evaluated or improve
12. register anything being sent or recieved To use the USB connection an additional program USBlib had to be downloaded that did not agree with the firmware version used We changed back to an older version of the firmware but this took away the possibility to compile and download new programs to the segway and we decided not to proceed with this So sadly enough we did not succeed in logging data which makes it hard to tell what the problem with our observer was 7 User Manual This section describes how to build the Simulink diagram and download the program to the segway A description of how to use the segway is also included There is also 7 USER MANUAL 3000 2000 F 3 1000 D 0 1000 0 5 10 15 20 25 30 D gt True Estimated 0 5 10 15 20 25 30 Time s a State one and two 500 v 2 0 5 ne D 500 0 5 10 15 20 25 30 v 2 5 me gt True Estimated 0 5 10 15 20 25 30 Time s b State three and four Figure 3 The true states blue and the estimated states red with our observer USER MANUAL 6 deg s bo 0 5 10 15 20 25 Time s Figure 4 The estimated offset of the gyro i e state five 30 7 USER MANUAL 9 another MATLAB toolbox available at 3 which is good if you only want to test the sensors No instructions for the second toolbox are presented in this report 7 1 How to use NX Tway GS The gi
13. ven controller in 1 uses the MATLAB toolbox Embedded Coder Robot for LEGO Mindstorms NXT or for short EC Robot NXT Information about the toolbox and how to install can be found in 2 All the files for 1 can be found on the desktop of the computer RTLT 12 in the directory Segway filer The MATLAB files can then be found under nztway_gs models The file natway_gs mdl is a simulation model that uses the controller in nztway_gs_ controller mdl The rest of this section focus on natway_gs_controller mdl since it is this file that is used on the segway The main window of the file nztway_gs_controller mdl can be seen in Figure 5 The controller is implemented in the subsystem natway_app Press the button Gen erate code and build the generated code to do what it says However there is some problem with the installation so the only thing that happens is that the Simulink diagram is built The rest has to be done manually according to e Open C cygwin bin bash exe e Go to the directory c Document and Settings rtadm Desktop Seqway filer natway_gs models natprj e Write make all e Write make rzeflash e Connect and start the segway e Write sh rzeflash sh The program is now on the robot and is called nztway_app which is the same as the Simulink subsystem described above 7 2 Start the Robot The main things to know about the robot is 1 Press the orange button to start the robot 2 Walk through the menus with the triangular buttons
14. w free to implement your on program The program should be implemented in the subsystem you just renamed i e terminator 7 4 Possible Error Messages You can find the solutions of some errors that may occur in this section e If Error executing callback ClickFcn Error using gt nxtbuild at 149 Failed to create nxtprj directory for model nxtway_app arises when you press the button Generate code and build the generated code then it is probably because you are in the nxtprj folder with Cygwin Go back one folder and try again e If you do not get the message filename rxe xxxxx when you type sh rze flash sh in Cygwin there might not be enough space on the NXT Delete pro REFERENCES 11 grams not used and try again It can also be that you have forgot to turn the robot on e If you get an error message saying Model failed to compile with strict bus check on Turning strict bus check off Model has compile errors when you have pressed the button Generate code and build the generated code then open the file rtwbuild Set a breakpoint at row 201 and then press the button Generate code and build the generated code again MATLAB will now stop at row 201 in rtwbuild and you have access to the variable newHac The variable is a MException object and contains information about the error The error message you are looking for can be found if you dig into the causes e g newExc causes 1 1 causes 1 1 message
15. way in an upstanding position with our observer during the simulation But when we download the observer to the real segway and run it the segway can not be controlled to an upstanding position We therefore tried to save the estimates from our observer and the existing one to see how different they are on the segway see Section 6 The different behaviour of the segway in simulation vs reality is probably because of the linearisation made in Section 3 the model and controller are linear but the reality is nonlinear One way to better imitate this situation would be to implement the nonlinear system in the simulation model to be able to tune the Q R matrices in eq 18 in a more suitable way Another solution would be to use a time varying Kalman filter instead of the time invariant KF we have used 6 Data logging The obtained observer works very well during simulations but not when compiled and used on the segway Therefore we wanted to log the measured data from the segway to better see what happens and why this did not work Our intention was to see how to tune the observer to the real segway i e choose new Q and R matrices that fit the true system better It is possible to connect the LEGO NXT brick to a computer using either Bluetooth or a USB cable and we started using the Bluetooth connection since this means the segway can move around freely like it is supposed to But we could not get the connection to work the COM port did not
16. y was constructed using a LEGO Mindstorms NXT kit and a gyro and the goal was to construct a self balancing segway To do this the motor angles and the gyro measurements were available and a working Simulink program The main focus in this project has been to construct an observer The segway can be used for demos in basic control courses and a manual can be found at the end of the report Nyckelord Keywords segway manual observer lego mindstorms

fulltext

Contents

Download Pdf Manuals

Related Search

Related Contents