Autonomous Robotic Satellite Capture Using Constrained Predictive Control by Richard McCourt B . A . S c , University of British Columbia, 2002 A T H E S I S S U B M I T T E D I N P A R T I A L F U L F I L M E N T O F T H E R E Q U I R E M E N T S F O R T H E D E G R E E O F M A S T E R O F A P P L I E D S C I E N C E in T H E F A C U L T Y O F G R A D U A T E S T U D I E S (Department of Mechanical Engineering) W e accept this thesis as conforming to the required standard T H E U N I V E R S I T Y O F B R I T I S H C O L U M B I A July 2004 © Richard McCourt , 2004 THE UNIVERSITY OF BRITISH COLUMBIA FACULTY OF GRADUATE STUDIES Library Authorization In presenting this thesis in partial fulfillment of the requirements for an advanced degree at the University of British Columbia, I agree that the Library shall make it freely available for reference and study. I further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. Richard McCourt 21/10/2004 Name of Author (please print) Date (dd/mm/yyyy) Title of Thesis: Autonomous Robotic Capture of a Satellite Using Constrained Predictive Control Degree: Master of Applied Science Year: 2004 Department of Mechanical Engineering The University of British Columbia Vancouver, BC Canada OCT 2 1 2004 grad.ubc.ca/forms/?formlD=THS page 1 of 1 last updated. 21-Oct-04 Abstract This thesis investigates the use of model-based predictive control for the capture of a multi-degree-of freedom object that moves in a somewhat arbitrary manner, using a deployable manipu-lator. While the study is conducted through both computer simulation and ground-based experi-mental investigation, the intended application is focused on automating the robotic capture of a free-floating and spinning satellite. The main motivation for this application comes from the fact that robotic satellite capture in space, for retrieval, correction, repair, etc., can significantly elimi-nate the risks involved when astronauts are used in space walk scenarios to manually execute the necessary tasks. When a satellite is spinning, the maneuvering of a robotic manipulator by a hu-man operator for a successful capture becomes increasing difficult. For this reason, satellite cap-turing using an autonomous robot is particularly attractive, and is studied in the thesis. The present investigation uses an innovative manipulator known as the Multi-module Deployable Manipulator System ( M D M S ) , which has been designed and built in our laboratory. It is a multi-purpose manipulator, which consists of a combination of revolute and prismatic joints and pro-vides several advantages over the standard, all-revolute-joint manipulators that are used in space applications. The advantages include reduced dynamic coupling, fewer configuration singulari-ties, easier obstacle avoidance, and simpler inverse kinematics, when compared to all-revolute-joint manipulators of the same size and degrees of freedom. Furthermore, our experience in de-signing, controlling, and experimentation with two models of the manipulator provides an added incentive for using the M D M S as the test bed for the present investigation. In the controller that is developed, computer-simulated, implemented, and tested in the present research, the future path of the target satellite is predicted, and the controller uses this future knowledge of the target, and an internal model of the manipulator, to make optimal control deci-sions to minimize the tracking error between the target and the end-effector. Multi-parametric Quadratic Programming (mp-QP) techniques are used in order to obtain constrained optimal con-trol decisions in real-time. The mp-QP algorithm offers fast solution times by explicitly solving the constrained quadratic programming problem offline and then using look-up tables in the real-time application. Unfortunately, the mp-QP solution is susceptible to numerical problems, and as a result the fully constrained predictive controller could not be implemented in real time. How-ever, a sub-optimal version of the controller was implemented. The results show that the mp-QP ii Abstract i i i algorithm is still able to realize user defined constraints; therefore providing the benefits of con-strained optimal control in the satellite capturing problem. The results show that when the satellite motion is predicted the tracking performance is im-proved. Moreover, when the physical constraints of the system are formulated into the optimiza-tion, the controller becomes aware of its own limitations and the approach towards the satellite is improved by eliminating possible overshooting of the target. Table of Contents Abstract 'i List of Tables vii List of Figures viii List of Symbols x Acknowledgements xiv CHAPTER 1: INTRODUCTION I 1.1 Background 1 1.2 Motivation 5 1.3 Literature Review 6 1.4 Thesis Outline 8 CHAPTER 2: PREDICTIVE CONTROL 11 2.1 Preamble 11 2.2 Receding Horizons 12 2.3 Control Horizons and Predictive Functional Control 13 2.4 Internal Models 16 2.5 Cost Function 17 2.6 Unconstrained Predictive Control 19 2.6.1 Controller Structure 19 2.6.2 Numerical Issues of MPC 23 2.7 Constrained Predictive Control 25 2.7.1 Constraint Formulation 26 2.7.2 Multi-parametric Quadratic Programming 28 2.8 Summary 30 CHAPTER 3: PROBLEM FORMULATION 31 3.1 Preamble 31 3.2 Dynamics of the Manipulator System 31 3.3 Target Kinematics 35 3.4 The Control System 39 3.5 Integration of Predictive Control 39 3.6 Integration of Constraints 42 iv Table of Contents v 3.7 Tuning the Controller 43 3.8 Summary 44 CHAPTER 4: SIMULATION STUDY 46 4.1 Preamble 46 4.2 Unconstrained Predictive Control 47 4.2.1 Effects of Prediction Horizon 47 4.2.2 Order of the PFC Basis Function 48 4.2.3 Location of Coincidence Points 48 4.2.4 Tuning of Weighting Matrices 49 4.2.5 Results from the Unconstrained Controller 50 4.3 Constrained Predictive Control 53 4.3.1 QP Solution •. 53 4.3.2 Mp-QP Solution 60 4.4 Summary 64 CHAPTER 5: EXPERIMENTAL IMPLEMENTATION 65 5.1 Preamble 65 5.2 Experimental Setup 65 5.3 Results from the Unconstrained Controller 67 5.4 Results from the Constrained Controller 70 5.4.1 Searching for the Optimal Control Law 71 5.4.2 Dealing with Infeasibilities 72 5.4.3 Experimental Results 72 5.5 Summary 75 CHAPTER 6: CONCLUSIONS AND SUGGESTIONS FOR FUTURE WORK 77 6.1 Conclusions 77 6.2 Suggestions for Future Work 78 Bibliography 81 APPENDIX A: Multi-Parametric Quadratic Programming (mp-QP) 84 A.l Preamble 84 A.2 Multi-Parametric Quadratic Programming for Model Predictive Control 84 A.3 Offline Algorithm 87 A.4 Online Algorithm 88 A.5 The Multi-Parametric Toolbox (MPT) of MATLAB 88 APPENDIX B: Dynamic Formulation of the MDMS 90 Table of Contents v i A P P E N D I X C : Further Results in Satellite Capturing Using Predictive Control 93 C. 1 Control Horizon M P C Results 93 C.2 More Experimental Satellite Capture Results 96 List of Tables Table 3.1 - Tuning parameters for the predictive controller 43 Table 4.1 - Results obtained with different target speeds 58 Table 5.1 - Unconstrained controller results for various conditions of target satellite 70 Table C . l - Steady state tracking error results from the control horizon form of the predictive controller 94 Table C.2 - Experimental results from various satellite captures 96 vii List of Figures Figure 1.1 - Robonaut 2 Figure 1.2 - Illustration of the M D M S on its space platform 3 Figure 1.3 - The M D M S prototype used in the experiments 4 Figure 2.1 - Performance comparison between M P C using a control horizon, M=2, and M P C using two polynomial basis functions 15 Figure 2.2 - Block diagram of the unconstrained predictive controller 22 Figure 2.3 - Block diagram of the constrained predictive controller 26 Figure 2.4 - Block diagram of the mp-QP controller 29 Figure 3.1 - Schematic diagram of the M D M S used for planar satellite capturing 32 Figure 3.2 - Feedback linearization of the M D M S 34 Figure 3.3 - A sequence of motion of a spinning satellite 36 Figure 3.4 - Block diagram of the predictive controller implemented on the M D M S 45 Figure 4.1 - The simulation model of robotic satellite capture 51 Figure 4.2 - Results from unconstrained controller showing the benefits of using predictions 52 Figure 4.3 - Results of input constraint realization with unconstrained controller tuning parameters in joint space 54 Figure 4.4 - Results of input constraint realization with unconstrained controller tuning parameters in task space 55 Figure 4.5 - Results from the re-tuned constrained controller with input constraints in joint space 56 Figure 4.6 - Results from the re-tuned constrained controller with input constraints in task space 57 Figure 4.7 - Results from the predictive controller under M D M S output constraints in joint space 59 Figure 4.8 - Results from the predictive controller under M D M S output constraints in task space 60 Figure 4.9 - Results from the mp-QP simulation in joint space 62 vi i i List of Figures ix Figure 4.10 - Results from the mp-QP simulation in task space 63 Figure 5.1 - M D M S used in the experimental investigation 66 Figure 5.2 - Initial configuration of the manipulator for the experiments 67 Figure 5.3 - Results from the unconstrained P F C in joint space 68 Figure 5.4 - Results from the unconstrained PFC in task space 69 Figure 5.5 - Constrained predictive controller results obtained using the prototype M D M S in joint space 74 Figure 5.6 - Constrained predictive controller results obtained using the prototype M D M S in task space 75 Figure B . l - Schematic diagram of the M D M S used for planar satellite capturing 90 Figure C . l - Block diagram of the unconstrained control horizon form of the predictive controller 94 Figure C.2 - M D M S joint-space tracking error response with', and without, predictions of the targets position 95 Figure C.3 - Joint space constrained predictive controller results for Case #1 97 Figure C.4 - Task space constrained predictive controller results for Case #1 98 Figure C.5 - Joint space constrained predictive controller results for Case #2 99 Figure C.6 - Task space constrained predictive controller results for Case #2 100 Figure C.7 - Joint space constrained predictive controller results for Case #3 101 Figure C.8 - Task space constrained predictive controller results for Case #3 102 List of Symbols A , B , C : State-space matrices describing the linearized and descretized model of the ma-nipulator; Equation (2.4). Ci: Vector of constraints on the optimal controller cost function at the future time, t = k + i; Equation (2.2). C(q,q) : Vector of Coriolis and centripetal force terms used in the description of the sys-tem dynamics {see Appendix B} . d2 : Deploy length of the second joint in the satellite capturing manipulator {see Fig-ure 3.1}. E Y E N : Matrix of N consecutive identity matrices; Equation (2.20) F s , F v : Diagonal matrices containing the Coulomb and viscous friction terms, respec-tively {see Appendix B} Fj, Gj: Gain and offset used to describe the affine control law in the region; Equation (5.2) Hj, K,: Define the z'th region convex polytope in the parameter space 9 i ' V / ' ; Equation (5.1) I: Identity matrix Ii : Moment of inertia of the z'"1 link k: Discrete time step, t = k-Tx K M pc : Feedback gain matrix used in the unconstrained predictive controller {Page 22}. K : Conversion factor from servo card voltage, u(k), to joint torque/force, T ( /C); Equation (3.2) lw : The length from the end-effector to the center for the third joint of the manipula-tor; Equation (3.4) / , : The distance from the grappling point on the satellite to it's center of mass; Equa-tion (3.5) M(0, R(z')>-0 Q, R : Weighting matrices in the vector-matrix form of the predictive controller cost functions; Equation (2.11). Q ^ O , R>- 0 S Q , S r : Cholesky decomposition of Q and R, respectively; Equation (2.17) Tx: Controller sampling time u(k) : Vector of servo card command voltages; inputs computed from the controller. u,{k) : Vector of z'th coefficient in the basis function description of the inputs u(/c) {Equation (2.3)} \u(k) : The change in control inputs, \u(k) = u(k) - u(k-\) AU(k): Vector of current and future changes in control inputs; Equation (2.10) V(k): The cost function minimized by the controller at each time step. Y , H , F : Matrix coefficients used in the description of the controller cost function; Equa-G , W, E : Matrices used to describe the constraints on the controller cost function; manipulator. Vector of joint positions [f5, d2 <93]' tion (3.9) Equation (2.20) Target state vector, xr [k] = [r{kf ,r(kf ,'r\kf f 17' V : Chaser robot output vector, y — x = Positive semi-definite List of Symbols xi i >- : Positive definite | R 2 : weighted 2-norm (weighted by the matrix R in this case) Greek Symbols Manipulator joint angles, / e {l, 3} Orientation of the target satellite X : Vector of Lagrange multipliers H,{k): Vector of /* coefficient in the basis function description of the input moves Au(k) {Equation (2.12)} fi(fc) : Vector of the M fu,{k) vectors and the minimization argument for the Predictive Functional Controller cost function; defined in Equation (2.12) ^(/c) : Vector containing the Np optimization parameters in the controller cost function; $(k) = [x(kf,u(k-l)r,xr(k)Tf T : Vector of joint forces and torques {see Appendix B} co : Angular speed of the target satellite *P, r, 0 : Matrix coefficients used in the description of the controller cost function {Equa-tion (2.10)} A : Transformation matrix from Xr(k) to T(k) {Equation (3.8)} T(k) : Vector of the current and future reference variables, r(k). Y(k): Vector of the current and future measured output, y(k). £1 : Transformation matrix from fi(k) to \U(k) {Equation (2.12)} Abbreviations, Acronyms A P P E C M G D A R P A HST: K K T : L P L Q R L Q G Active Prediction Planning and Execution Control Momentum Gyro Defense Advanced Research Projects Agency Hubble Space Telescope Karush-Kuhn-Tucker Linear Programming Linear Quadratic Regulator Linear Quadratic Gaussian List of Symbols X l l l M D M S Multi-module Deployable Manipulator System M P C Model Predictive Control mp-QP multi-parametric Quadratic Programming M P T Multi-Parametric Toolbox M S S Mobile Servicing System N A S A National Aeronautics and Space Administration N A S D A National Space Development Agency of Japan O S V Orbital Service Vehicle PFC Predictive Functional Control QP Quadratic Programming R H C Receding Horizon Control R T X Real-Time extension S C A R A Selective Compliant Assembly Robot Arm S P A R T A N Shuttle Pointed Autonomous Research Tool for AstroNomy A dot above a character refers to differentiation with respect to time. A boldface italic character denotes a vector quantity. A boldface character denotes a matrix quantity. A ' A ' above a character denotes an estimation of that variable. Subscripts 'e' and' t ' correspond to the manipulator end-effector and target, respectively. Subscripts's' and 'd ' refer to the slewing and deploying links, respectively. Acknowledgements I would like to thank my supervisor, Professor Clarence W. de Silva, for providing financial as-sistance, suggesting the research topic, and subsequent supervision and guidance of my research, which has led to the satisfactory conclusion of the present thesis. Also, I wish to thank Professor Vinod J. Modi who jointly with Professor de Silva, accepted me for the Master's program. Thanks to Professor Elizabeth A . Croft and the rest of the Industrial Automation Laboratory for the knowledge and enlightenment shared in those morning meetings. Thanks are also due to my colleagues and friends: Dr. Yang Cao, Mr. Parry Fung, Ms . Dana K u -lic, M r . Masaki Ohmiya, Mr . Will iam Owen, Ms. Nazly Pirmoradi, Mr . Zhimin Q i , Dr. Keel-Soo Rhyu, M r . Tao Sang, Mr . Poi Loon Tang, Mr . Y i n g Wang, Mr . Duminda Wijewardene, Mr . Ken-neth Wong, and M r . Jian Zhang. I greatly appreciate the knowledge, experience and culture that they have shared with me. Thanks to my parents, Stephen and Jenny McCourt, my sister, Kristy McCourt, and the rest of my family for all of their support and encouragement that they have given me. And last, but certainly not least, my thanks go to Tracy Ma , for her love and kindness, and for helping me through the good and the bad times. Funding for this project has come from grants to Professor Clarence W. de Silva from the Natural Sciences and Engineering Research Council (NSERC) of Canada. xiv CHAPTER 1: INTRODUCTION 1.1 Background One of the many reasons for the development of robotic manipulators is the ability for them to autonomously perform tasks that are not particularly suitable for humans; for example those in-volving long and repetitive operations and unhealthy, unpleasant and hazardous environments. Due to the particularly harsh environment of space, the application of robotics has received sig-nificant attention. The associated research activity and developments have led to many advances in space robotics that can be used to aid astronauts in performing extravehicular activities as well as numerous tasks related to the construction and operation of the International Space Station. Moreover, increased efforts have been made to develop autonomous and telerobotic systems that can be deployed in space while the human operators remain on Earth, thus eliminating some of the major risks involved in current launching procedures and also resulting in possible reductions of mission costs. A n important issue concerning the presence of humans in space is the servicing of the large num-ber of satellites that orbit Earth. A special-purpose satellite that has received a considerable amount of attention is the Hubble Space Telescope (HST). In the past, astronauts, with the help of the Canadian robot, Canadarm, have been sent on board the Space Shuttle to perform various repairs on Hubble in order to keep it operational. However, recent efforts to repair the telescope have been halted due to increased safety concerns after the loss of the STS-107 crew in the space shuttle Columbia. Efforts to save the space telescope have led to heightened interests in the area of space robotics, with the emphasis on telerobotics. A prime candidate for Hubble servicing ro-bots is a tele-operated android named Robonaut, designed by the Robotic Systems Technology Branch of N A S A , in collaboration with the Defense Advanced Research Projects Agency ( D A R P A ) of U S A [Ambrose, et al., 2000; Bluethmann, et al., 2003]. 1 Chapter 1 2 Figure 1.1 - Robonaut The interest in Robonaut goes deeper than the maintenance of the HST. The successful robotic servicing of the Hubble telescope wi l l undoubtedly give a further boost to the growing interest in unmanned orbital service vehicles ( O S V s ) . These OSVs would be capable of remaining in orbit for very long periods of time and sufficiently versatile to service a large variety of orbiting space traffic. MacDonald, Dettwiler and Associates ( M D A ) and Boeing Phantom Works are doing some work in this area with their "Orbital Express" program, and so has the National Space De-velopment Agency of Japan ( N A S D A ) with their Hyper-OSV [Matsumoto, et al., 2002]. Control of the Robonaut wi l l be achieved through human tele-presence control systems, allowing it to be operated safely from a ground control station on Earth. Unfortunately, the communica-tion delay between Robonaut (or any future OSV) and the ground control station can be on the order of several seconds. This delay would add to the difficulty that a remote operator would face in maneuvering a manipulator in a complicated task, not the least of which is system instability. One task that has proven to be particularly difficult in the past is the maneuvering of a robot in the capture of a moving target. Clearly, the difficulty increases manifold when the future trajec-tory of the target is incompletely known. Several satellites have been captured for servicing or repair with the help of the Canadarm robotic manipulator on the space shuttle; however, the task can become extremely difficult when the satellite is spinning or tumbling out of control. For ex-ample, during STS-87, an astronaut operating the Canadarm attempted to capture a free floating S P A R T A N satellite. In this attempt the end-effector of the manipulator nudged the target satellite, causing it to tumble, thus making it more difficult to complete the capture, so, astronauts were Chapter 1 3 sent out on a space walk to capture the satellite by hand. O f course, in the case of Robonaut, or other unmanned OSVs, human aid wi l l not be utilized. Automating the satellite-capturing task would reduce the risks associated with these operations, reduce the mission costs, increase the likelihood of successful capture, and generally improve the effectiveness of the O S V . Some research has been done in the area of automated satellite-capturing, but so far the only autonomous satellite capture performed while in orbit has been done on N A S D A ' s Engineering Test Satellite (ETS) VII [Oda, 2000]. In this case, the robotic manipu-lator on the chaser satellite removed and released a target satellite from its base and then success-fully recaptured it. If the future of space robotics rests on tele-operated and autonomous systems, then autonomous moving target capturing should be included in the repertoire of the manipulator's abilities. One such manipulator that possesses several advantages over standard all-revolute-joint space manipu-lators is the Multi-module Deployable Manipulator System ( M D M S ) as conceived in our labora-tory. Multi-module Deployable Manipulator (MDMS) Orbit Figure 1.2 - Illustration of the MDMS on its space platform. Chapter 1 4 A n illustration of the orbiting space platform based concept of this manipulator design is shown in Figure 1.2. The M D M S is comprised of a series of two-degree-of-freedom modules connected together in a chain topology. Each module is a combination of one slewing (revolute) link and one deploying (prismatic) link. The manipulator may be configured for a particular application by connecting as many modules as necessary. The M D M S has a variable geometry structure, in view of its deploying links, which provides particular advantages in obstacle avoidance. It has been designed as a multi-purpose manipulator, similar to the Mobile Servicing System (MSS) on the International Space Station; however, compared to conventional (all revolute joint) manipulators, like the M S S , the M D M S has several advantages. In addition to better obstacle avoidance, the M D M S has fewer singular configurations, reduced dynamic coupling between links, and simpler inverse kinematics. Figure 1.3 - The MDMS prototype used in the experiments, A ground-based prototype of the M D M S as developed in our laboratory is used in the experimen-tal investigations of the present research. In particular, a model-based predictive controller wi l l be implemented on the M D M S , and its real-time performance wi l l be investigated. The physical pro-Chapter 1 5 totype used in our experiments is a planar manipulator with rolling supports under the manipula-tor joints, to eliminate the gravity effects. The M D M S prototype is shown in Figure 1.3. The revolute joints of the manipulator use DC motors fitted with low backlash, harmonic drive gears, and deployment in the prismatic joints is provided by D C motors directly coupled to ballscrew actuators. Feedback motion signals from the joints are sensed using incremental optical encoders attached to the motor shafts, and fed to a servo card installed on the robot control computer run-ning Windows 2000. The controller is programmed in Visual C++, and VenturCom's Real-Time Extension ( R T X ) provides the application programming interface (API) thereby giving real-time capabilities to Windows [de Silva, McCourt & Ohmiya, 2003]. 1.2 Motivation The investigation presented in this thesis focuses on the use of model-based predictive control for robotic capture of an object whose motion is not completely known. The main application con-sidered is satellite capture using an autonomous robot for motion correction, retrieval, and repair. A multi-module deployable manipulator system ( M D M S ) as developed in our laboratory wi l l be used as both the analytical/computer model and the experimental prototype in the present investi-gation. In this section, the motivation and justification wi l l be provided for the use of M D M S and model-based predictive control in the present investigation. The M D M S is a multi-purpose space manipulator with several advantages over conventional ma-nipulators, and the satellite capture problem is investigated here as a specific application of this class of manipulators. The autonomous satellite-capturing controller must be able to smoothly maneuver the manipulator towards an expected interception point with the target and continue to track the target until the capture is complete. Since the target wi l l be moving with incompletely known motions, it is desirable to have a controller that is capable of anticipating the future movement of the target. In addition, i f the controller is aware of its own capabilities it can use this knowledge, and any available knowledge of the target, to help in choosing the best robotic action. Model Predictive Control (MPC) is a model-based optimal controller, which uses open loop pre-dictions of the system response into a finite, future length of time to repeatedly minimize a user-defined cost function, in achieving the specific control objective. Predictions can be made on Chapter 1 6 both the target and the robot to create the desired anticipative control action. The specific predic-tive controller used in the present work has the ability to handle user-defined constraints. These constraints can be used to specify the desired level of smoothness in the joint motions. For these reasons M P C is particularly suitable for use in the autonomous satellite capturing robots. M P C has been successfully used in the process control industry; however, the application of pre-dictive control in the aerospace industry appears to be relatively new. The recent applications of predictive control to high bandwidth aerospace systems have been aided by work in muti-parametric, explicit M P C solutions [Bemporad, et al. 2002] and with the constantly increasing speed of computers. Several control techniques have been implemented on the M D M S in our laboratory, for trajectory tracking problems. Noteworthy are linear quadratic regulator (LQR) , linear quadratic Gaussian (LQG) control, feedback linearization technique, modal control, intelligent fuzzy control, and neural-network adaptive control. These techniques, however, do not possess the "predictive" ca-pability as useful in the satellite capture problem. The recent developments in efficient M P C so-lutions and the ever-increasing speed of digital computers have made possible the effective use of a real-time constrained model predictive control in high-bandwidth applications such as the M D M S . Satellite capture provides a good motivation for this implementation. 1.3 Literature Review Autonomous satellite capture using robotic manipulators has been done before even though this is a relatively new topic. In particular, the ETS-VII satellite has performed an autonomous satellite capture while in orbit [Inaba & Oda, 2000]. A brief literature survey is given now to present some existing work in the areas of satellite catching and robotic predictive controllers, in the con-text of the present investigation. In the area of satellite capturing, a focus has been the development of vision algorithms that are capable of supplying reference paths to standard robotic controllers [Jasiobedzki, et al., 2001]. Pose determination and machine vision are not considered in the present investigation, and it is assumed throughout the thesis that the current position of the target object is known. Chapter 1 7 In order to study the satellite capturing, it is important to understand how the satellite wi l l be moving and how the manipulator can be used to capturing the satellite. In earlier work on the M D M S , Ueno and Modi [1998] considered the satellite capturing problem in computer simula-tion, where the satellite was assumed to be a point object, moving with constant velocity in a two degree-of-freedom environment. The difficulty in capturing a satellite is increased when it is spinning or tumbling. This is because typically there is a specific grappling point where the manipulator must make contact with the satellite, and when the satellite is spinning, the grappling point is rotating around the satellite's center of mass. Nagamatsu, et al. [1996] studied the motion of a tumbling satellite and described this motion as the superposition of three rotations with constant angular velocities. First, the magnitude of these angular velocities was estimated. Then a manipulator with enough degrees of freedom aligned three joints with the three axes of rotation, and maintained alignment by spin-ning these joints at the same velocity as the corresponding axes. When done properly, the captur-ing of the satellite became simple because the relative velocity between the grappling fixture and the end-effector of the manipulator was very small. In later studies, Nagamatsu [1997] considered a target that was translating as well as rotating, and predictions of the target position and orientation were used to compensate for time delays due to image processing. The position and orientation of the target were corrected and used as the de-sired position for a Jacobian inverse controller. Control of the manipulator was not considered further. Richards and How [2003] have used M P C to autonomously guide a spacecraft towards a rendez-vous point (e.g. satellite). The paper offered a guaranteed robust, finite time maneuver comple-tion for an arbitrary target region. The work solved a minimum fuel consumption problem with added constraints to guarantee a successful rendezvous with the target. The M P C controller pro-duced solutions that were more robust against unmeasured disturbances when compared to previ-ously used control schemes for this problem. In addition, the use of parametric programming techniques allowed for more complicated cost functions to be minimized, and overall improved the performance of the maneuver. Predictive control has been applied to a variety of mobile robotic applications, but fewer applica-tions have been made on robotic manipulators. This is most likely because of the added difficulty Chapter 1 8 in dealing with the nonlinearities of the manipulator. Some robotic predictive controllers first linearize the robot using feedback of the inverse dynamics of the manipulator [Poignet & Gautier, 2000; Torres et al., 2001], while others have used Taylor series approximations [Hedjar et al., 2002] or linearization of the robot at each time step [Valle, Tadeo & Alvarez, 2002]. Poignet and Gautier [2000] made a comparison between the commonly used Computed Torque Control [Sciavicco & Sicilano, 2000] and a variation of M P C often used for high bandwidth sys-tems known as Predictive Functional Control (PFC). In both controllers the plant is first lin-earized with the inverse dynamics model of the manipulator. Simulation results on the first two links of an industrial S C A R A type robot showed that, in the absence of disturbances or model uncertainties, the computed torque controller could be tuned to give better tracking performance. However, the predictive functional controller performed significantly better in the presence of unmeasured disturbances and model uncertainties. Valle, et al. [2002] applied constrained predictive control to a two degree-of-freedom direct-drive robot. A linearization of the manipulator was performed at each time step and then the con-strained optimization of the predictive controller cost function was computed. O f course, this method was very computationally intensive, and even on a two degree-of-freedom robot with a very small prediction horizon, only computer simulations could be performed. Typically, in the literature, good simulation results appear to have generated when predictive con-trol was applied to a robot, but such claims have not been validated for the most part through ex-perimental investigation and practical application. In the past work, M P C has been used to guide spacecraft towards satellites, and predictive control has been used on robot manipulators. The work presented in this thesis integrates such efforts in predictive control of robotic satellite cap-turing with the M D M S , using experimental implementation and verification as well as analysis and computer simulation. 1.4 Thesis Outline In this introductory chapter the objectives of the present investigation are presented, the motiva-tions are given, and the approach used in achieving the objectives is outlined. In particular, a need for autonomous robotic satellite capturing has been presented, along with some of the existing Chapter 1 9 work in this area. The multi-purpose M D M S robot as developed in our laboratory has been pre-sented as a suitable application test platform for autonomous satellite capturing. Two prototype units of the M D M S have been developed in our laboratory. One is available to serve the role of the robot in autonomous satellite-capturing experiments, and the other unit as the moving satel-lite. Model-based predictive control wi l l be implemented on the M D M S for autonomously controlling the capture of a spinning satellite. Predictive control has been chosen here because of several pertinent advantages; in particular, it is an optimal controller that can be made to anticipate the future movements of the satellite target, under realistic constraints, and make improvements to the capturing process. In Chapter 2 the theory behind model predictive control, as used in the present work, is presented. This chapter shows the general structure of the predictive controller and discusses how future set-points and constraints can be incorporated into the control decision. Then, a brief introduction to multi-parametric quadratic programming techniques is presented to show how the predictive con-troller can be implemented in high-bandwidth applications like the M D M S . In Chapter 3 the autonomous satellite capturing problem is formulated. The dynamics of the M D M S and the target satellite are studied so that reference models of these systems can be incor-porated into the controller. Next, the details of the integration of predictive control for the M D M S are discussed. The controller developed in this chapter is needed in the computer simula-tions and real-time experimentations with the M D M S , as presented in chapters 4 and 5, respec-tively. In chapters 4 and 5 the predictive controller that is developed in Chapter 3 is used in robotic cap-ture of a variety of satellite targets. In Chapter 4, in particular, the M D M S is simulated, and the optimization of the constrained predictive controller is performed at each time step to initially demonstrate the benefits of formulating constraints into the predictive controller. In Chapter 5, off-line solutions to the predictive controller optimization problem are computed in order to im-plement the constrained predictive controller at high control update rates, which are needed for the prototype M D M S . Chapter 1 10 The main conclusions of the work are given in Chapter 6, followed by some suggestions for fu-ture work in satellite capturing using predictive control on the M D M S . CHAPTER 2: PREDICTIVE CONTROL 2.1 Preamble The predictive controller that is implemented in the multi-module deployable manipulator ( M D M S ) is an optimal controller, which is based on a predicted future response. It is known for its ability to operate close to state constraints, compared to a conventional controller [Macie-jowski, 2002]. A typical optimal controller minimizes a user-defined cost function based on a model of the plant and a set of boundary conditions. The resulting optimal controller is obtained by solving the Hamilton-Jacobi-Bellman equation, dV / » d V ' , \ _ + V(x,u,t) + 2^—- gj(x,u,t) = 0 dt dXj (2.1a) Here, V(x, u, t) is the user-defined cost function, and V is the value of the cost function at the optimal control input, u = u'eU, where U is a constrained subset of the /vdimensional space defined by the vector u, U c. W" . The vector g(x,u,t) describes the system dynamics as given by the set of n state equations x - g(x, u,t) , where x is the order state vector. The optimal control input can be expressed as a function of the current states; i.e., as a state feed-back control law. The optimal control law is given by the minimization of equation (2.1a), as u(x,t) = arg mm V(x, u,t) + X ^ T ^ / (*> "'0 (2.1b) Unfortunately, the partial differential equation (2.1a) is very difficult (and often impossible) to solve, so as to yield an explicit solution for (2.1b). Only in special cases can a solution be deter-11 Chapter 2 12 mined; for example, HyH,*, control or the Linear Quadratic Regulator (LQR) , which are computed in the absence of inequality constraints. When inequality constraints are added to the problem the solution becomes a piecewise defined control law, and the explicit solution becomes further complicated. Only special cases of these problems, for example, the minimum time control, have been solved. Anti-windup schemes can be added to the standard optimal controller as a type of ad hoc solution to integrator windup when constraints are met, but the optimality of the solution is not guaranteed, with respect to the cost function. Model Predictive Control (MPC) , on the other hand, provides a more formal solution to a constrained optimal control problem, where the constraints are explicitly taken into considera-tion, thereby reducing the possibility of constraint violations. In the rest of the chapter gives an introduction to model predictive control (MPC) . Only the the-ory that is relevant to this thesis is covered. First, the basic moving horizon is described. Next the formulation of both the unconstrained and the constrained model predictive controllers is pre-sented. More details on the topic are found in [Maciejowski, 2002]. 2.2 Receding Horizons In M P C the optimal control problem is solved in each control period. A cost function is mini-mized based on open-loop predictions of the plant response and subject to user defined con-straints. Naturally, this wi l l significantly increase the computational effort of the controller, so, depending on the application, certain efficiencies have to be invoked or simplifications have to be made in the M P C algorithm to make the computational time feasible. One possible simplification is to only perform the optimization over a finite time horizon of N future time steps and make this horizon sufficiently small so that the computations can be made within the control period, Ts. A set of Noptimal control input vectors (u(k \ k)^, w(/t + l | A:), . . . , u(k + N-l | k)) for the cur-rent and the future time instants k, k+1, N-l are determined, based on the current knowledge and the control objective, by minimizing the cost function * The notation u (k +11 /c) means the predicted control input at time t=k+i based on measurements (or, knowledge) at time t= k Chapter 2 13 where: k+N V{k) = X /(*('' I I *)> "O"-1! *)>') (2-2) subject to c, (£(/'1 /c), r(; | &),«(/'| /c), /) < 0 , and jc(/1 A;) = g ( j e ( / - l | A:), « ( / - ! | A:)) = total cost at current time, t=k, f[x[i | A;) , f (z1 A : ) , « ( / - l | A:),/) = cost at the future time, f=7, Cj 1 k},r(i | A:),«(/1 A:),/) = inequality constraints at future time, t=i, N = prediction horizon, and the state model x(k +11 /c) = ^(JC(A: | A:), u(k | A:)) describes the system dynamics. Once the optimal solution has been determined, the corresponding input = M ( / C | /C) is ap-plied to the plant. The process is repeated at the start of the next control cycle. From equation (2.2) it can be seen that for a constant N, the terminal time, k+N, increases with k. This is known as a receding horizon, and because of this M P C is sometimes referred to as Receding Horizon Control (RHC). 2.3 Control Horizons and Predictive Functional Control As mentioned earlier, the computation time of the predictive controller is a major limiting factor in the real-time implementation of M P C in high-bandwidth processes. Even with a fairly small prediction horizon the optimization problem can still be rather computationally intensive. As a further effort to decrease the computational load, control horizons are introduced. When a control horizon M i s introduced to the M P C algorithm, the cost function (2.2) is reformu-lated so that for some M k+M. Specifically, the optimal solution consists of only M input vectors (w(/c|/ t), M ( / C + 1 | / C ) , Chapter 2 14 u(k + M-\ | £ ) ) , and the remaining N-M input vectors are all identical; i.e., u (k + /1 /c) = u (/c + M -11 k) for M < i < N. The value of the control horizon M can then be chosen to provide a more desirable sized optimization problem without having to place heavy restrictions on the prediction horizon, N. The control horizon, M, also has some effect on the performance of the controller. The prediction horizon, N, determines the number of variables that are penalized in the cost function. When a control horizon is used, the same number of parameters must be minimized in the cost function, but now there are fewer degrees of freedom to choose from, and this wi l l affect the solution of the optimization. Exactly how it affects the solution wi l l depend on the system model. Typically, as the control horizon is decreased the controller becomes more aggressive. Instead of using only M control moves in the beginning of the horizon, a more intuitive solution is to use a linear combination of M basis functions to describe the inputs throughout the horizon. Any basis functions could be used, but in this thesis a polynomial basis is chosen. Now, the cur-rent and future input vectors are related through an (M-l)-order polynomial function, u(k + i | /c) = u0 + ( / c ) z H — + « M . ( ( i ) i M " ' , (2.3) and the M vectors «#(k), «/(k), uM.j(k) become the argument of the original minimization problem in equation (2.2). The basis function parameterization of M P C is known as Predictive Functional Control (PFC) and was originally developed by Richalet, et al [1978, 1987] in the early 1980s for application of M P C on high-bandwidth servomechanisms. P F C has several advantages over the control horizon form of the controller. In general, PFC is capable of making more accurate predictions for a better-fit solution to the minimization problem in (2.2), especially when tracking complex trajectories [Qin & Badgwell, 1997]. This is demon-strated in Figure 2.1 where an arbitrary setpoint trajectory was given to a simulated prismatic joint of the M D M S described in Chapter 1. A comparison is made between M P C with a control horizon of two and P F C with the future inputs described by two basis functions (a step and a ramp). A simple quadratic cost function penalizing tracking errors and control input changes was used. A further discussion on cost functions wi l l be given in a subsequent section of the thesis. Chapter 2 15 Figure 2.1 - Performance comparison between M P C using a control horizon, M=2, and M P C using two polynomial basis functions. Chapter 2 16 As shown in Figure 2.1, the tracking performance of the two controllers is almost identical. The major difference between the two controllers can be seen in the evaluation of the defined cost function. On average, the cost value of the control horizon form is 15,000 times larger than that of the basis function form. The more optimal solution has led some researchers to believe that PFC may provide an advantage when controlling nonlinear systems [Qin & Badgwell, 1997]. Moreover, by adjusting the cost function in the two controllers it was observed that PFC provided a more robust solution. The cost function parameters were adjusted until the control horizon for-mulation became unstable, yet the PFC controller maintained a very similar solution. Unfortunately, the advantages of P F C do come at a cost. One disadvantage of PFC is that it tends to be more susceptible to numerical problems when trying to minimize equation (2.2). Another problem is that i f the order of the input function is made too large, the calculated optimal solution can become rather oscillatory. 2.4 I n t e r n a l Mode ls Model predictive control (MPC) uses a linear state-space model of the process in determining the control action. O f course, not all plants can be represented by linear models, and some effort has been made in extending M P C to nonlinear models [Qin & Badgwell, 1998]. However, the vast majority of developments and applications of M P C concern linear models. In fact, using a linear model makes it easier to solve the minimization problem in (2.2), and moreover, i f the cost func-tion is chosen properly, it is possible to show that the minimization returns a unique, global solu-tion. In this thesis a linear internal model is used in the implementation of M P C . Since the plant is a robotic manipulator, which is known to possess a variety of nonlinearities, some type of lineariza-tion must be done before M P C can be implemented on it. Details on how the plant is linearized and how the linear model is obtained wi l l be discussed in Chapter 3. For now, the linearized, dis-crete-time, state-space model that is employed is expressed as Chapter 2 17 x(k + \\k) = Ax(k\k) + Bu(k\k) y(k\k) = Cx(k\k) ( 2 ' 4 ) where x is the ^-dimensional state vector, u is the «„-dimensional input vector, and y is the ny-dimensional output vector. The rest of this chapter assumes that a linear model of this form is available for the robotic system. 2.5 Cost Function The cost function of the optimal controller penalizes predicted reference tracking errors and the corresponding control decisions. Typically the 1, 2, or co-norm of the reference errors and control decisions are used. It can be shown that i f the cost is formulated with one of these three norms there exists a unique, global solution to the minimization problem in equation (2.2) [Fletcher, 1987]. The 1 and co-norm cost functions are linear, and since a linear model of the plant is used, both of these forms lead to Linear Programming (LP) problems. The advantage to using a linear cost function is that there are many well known, and fast, algorithms for the L P optimization problem. Additionally, the co-norm cost function only penalizes the maximum predicted tracking error and tends to make the controller more robust against unmeasured disturbances [Maciejowski, 2002]. However, the disadvantage of using a linear cost function is that, as is well known [Fletcher, 1987], the solution to an L P problem always lies at the vertex of the polytope defined by the con-straints, C; (jc(/|jfc),r(i ' | * ) ,« ( / | J fc ) ) in (2.2).' This means that at all times nu of the constraints wi l l be active. With a quadratic cost function, the minimization in (2.2) becomes a Quadratic Programming (QP) problem, and it is possible to find an unconstrained solution. Furthermore, the quadratic cost function can be formulated with weighting variables, which can be adjusted to affect the output performance, whereas in L P the constraints must be adjusted to affect the performance. In this thesis a quadratic (i.e., 2-norm) cost function is used. The cost function minimized in the tracking control problem is Chapter 2 18 N r-nk) = ^ \\y{k + i\k)-f{k + i\ k)\\w) +1 A i (k + i -11 * ) | r ( 2 . 5 ) where || y [k + /1 /c) - f [k + i | || is the 2-norm of the tracking error at the future time t=k+i weighted by the diagonal and positive semi-definite weighting matrix C»(/) ^ 0 . The changes in control inputs, A H (k + i | , are penalized in the cost function and the amount at which they are penalized at time /=/£+/-1 is weighted by the diagonal and positive definite matrix R(/) >- 0 . This is the most commonly used form of the quadratic cost function. The control increment Aw (/c + /1 £ ) is chosen as an argument in the cost function, as a means of providing offset free tracking when the steady-state control input is expected to be constant. However, i f the steady state input is expected to be a ramp, then the second difference, A 2 « (k + i | A) = A M {k + i \ /c) - A M (/C + / -11 /c), should be used in ( 2 . 5 ) , and the third difference, A 3 M (k + i | A:), i f the input is expected to be parabolic, and so on. For the position control of the M D M S when tracking a moving target the cost function form in ( 2 . 5 ) is adequate. This aspect wi l l be revisited in Chapter 3 . In addition to providing offset-free tracking, the particular choice of the decision variable can also improve the smoothness of the control input." When the first difference, Au(k), is used then the control input to the plant is u(k) = u(k-\) + Au(k) ( 2 . 6 ) On the other hand, when the second difference is used, we have A2u(k) = Au(k) - Au(k - 1 ) = ii(jfc) - u(k - 1 ) - u(k - 1 ) + u(k - 2) or, u(k) = 2u(k-\)-u(k-2) + A2u(k) ( 2 . 7 ) Chapter 2 19 It follows that, increasing the order of the input difference is equivalent to increasing the order of a low-pass filter applied to the control input. However, applying a filter can affect the stability of the system, especially when dealing with high-bandwidth servomechanisms like a robot manipu-lator. In view of this, care should be taken when choosing the argument of the cost function. The results in chapters 4 and 5 show that in the satellite capturing problem the combination of P F C with a cost function argument of A M (k + i \ k) generally produce smooth input functions and ac-curate tracking, hence, justifying the specific cost function given by (2.5). 2.6 Unconstrained Predictive Control 2.6.1 Controller Structure Now that the cost function and the internal model have been defined, methods can be developed for solving the optimization problem. To start, the unconstrained controller is formulated. Spe-cifically, the optimization problem in M P C becomes (2.8) r 2 2 Minimize V(k) = ^ jy(k + i\k)-r(k + i\ k)j . +|A«(A + J'-1 | k)j Subject to y(k + i\k) = Cx(k + i\k) = CAx(k + i-\\k) + CBu(k + i-\\k) In this formulation (unconstrained), the process model is treated as the constraints of the optimi-zation problem, as clear from (2.8). Since the cost function is formulated with Aw(k + i\k), the constraints (i.e., the model) should also be formulated with Au(k + i \ k). This can be done by setting u(k + i\ k) = «(£-!) + A«(£| k) + Au(k + \ \ k) + --- + Au(k + i\k) (2.9) By using this new formulation of the linear model, the predicted outputs of the plant can be writ-ten in the matrix form as Chapter 2 20 " .v(*+l|*)~ " C A " .V(k+2\k) C A 2 y(k+3\k) = C A 3 *(*) + y(k+N\k) _ C A W _ C B C A B + C B C A 2 B + C A B + C B Z S ' C A ' B u(k-l)^ C B 0 C A B + C B C B ZLiCA 'B C A B + C B 0 0 C B Y(k) Z M C A ' B Efi 'CA'B Z S ' C A ' B Q AH (A|*) AH(*+1|A) Au(*+2|i) (2.10) C B ||_Att(A+AM|*)_ AU(k) N o w the constraints in (2.8) can be incorporated into the cost function through direct substitution, and the new cost function can be rewritten as V(k) ^\\xVx(k) + Tu(k-\) + &AU(k)-T(k)fQ + \\AU(k)(R = [¥x(k) + ru(k-\)-T(k)i' Q[Wx(k) + ru(k-\)-T(k)] + 2AU(kf 0TQ[yi'x(k) + ru(k-\)-T(k)] + AU(kf[0TQQ + RJAU(k) where: ~ r{k + \\k)~ ~Q(V 0 0 0 r(k + 2\k) 0 Q(2) 0 0 T(k) = r(k + 3\k) Q = 0 0 Q(V • 0 r(k + N\k) 0 0 0 • Q(N) R(\) 0 0 0 0 R(2) 0 0 0 0 R f 3 ) • 0 0 0 0 • R(N) Note that an alternative means of incorporating constraints into an optimization problem would be through the use of Lagrange multipliers, but this increases the order of the overall optimization problem. Chapter 2 21 To get the final form of the cost function, the / V terms of A M are replaced by an ( A / - l )" ' -order polynomial function. AU(k) = I 0 0 0 MB (K) I I I I Mi [k) I 21 41 2M'll I (tf-l)] [ (N-lfl • • ( T V - i f - ' I MM-j{k) il /*(*) (2.12) where I is an w„ t h-order identity matrix, and the [i,{k)s are the coefficients of the polynomial basis function describing the future changes in control inputs, A M ( / C + / | / C ) . Equation (2.12) can now be substituted into equation (2.11) to give V(k) =[Wx(k) + ru(k-\)-T(k)]' Q[y¥x(k) + Tu(k-\)-T(k)] + n(k)~YT \j!x(k) + Tu(k-\)-T(k)\ (2.13) + n(k)' H fi(k) where F = 2 Q 0 f t and H = il1 [&TQ® + R]il. Equation (2.13) is the P F C form of the cost function and is the form used in the present thesis for the tracking control problem of the M D M S . The optimal polynomial function comes from the minimization of this cost function. In equation (2.5) the tracking error weighting matrices, Q(/), are defined as positive semi-definite matrices, and the control move weighting matrices, R(z'), are defined as positive definite matrices. This makes the Hessian in equation (2.13) positive definite (because H = il and therefore, the cost function, V(k), is convex. This means that the optimal solution is both unique and global [Fletcher, 1987]. Equation (2.13) is now minimized in the absence of con-Chapter 2 22 straints. This can be done as usual, by taking the gradient of the cost function and setting it equal to zero. VflV(k) =FT [*¥x(k) + ru(k-\)-T(k)] + 21I fii(k) = 0 (2.14) ^ ( * ) = 2 H " ' r [T(k)-Wx(k)-Tu(k-\)] (2.15) In the unconstrained case of M P C , the optimal solution in (2.15) is a function of the current state, the previous input, and the desired setpoints. A feedback matrix, K M pc , can be computed offline in order to achieve fast implementation online. A block diagram of the controller is shown in Figure 2.2. y(k) = x(k) Figure 2.2 - Block diagram of the unconstrained predictive controller. Unfortunately, inverting the Hessian in (2.15) can be rather difficult because M P C problems are known to be naturally i l l conditioned. It is important to deal with these numerical issues early because the condition number of the Hessian directly affects the performance of the controller and wi l l cause further problems when constraints are added to the optimization. Chapter 2 2.6.2 Numerical Issues of M P C 23 If the condition number of the cost function Hessian is large, then the numerical accuracy of (2.15) wi l l be lost due to round off errors. In the worst case, i f the condition number is too large, then the Hessian cannot be inverted and (2.15) wi l l not provide a solution. Another way to see how the Hessian affects the performance of the controller is to look at the Hessian for the cost function in equation (2.13), Here, Q and R are matrices containing the current and the future weights on the tracking errors and control input moves, respectively. It is easy to see how these matrices directly affect the Hessian, and therefore, the performance of the controller. For the case of the unconstrained controller some of the numerical problems can be avoided by formulating the optimal solution as a least-squares problem. Maciejowski [2002] shows that the quadratic cost function is equivalent to the squared length of the vector in where SQ TSQ = Q and SR TSR = R. In view of this, by setting the length of this vector equal to zero, one can minimize the quadratic cost function. This can be accomplished by setting the vector itself equal to zero and then solving for //(k). By setting the vector equal to zero rather than the length of the vector, unnecessary squaring of the ill-conditioned matrices © and Q is avoided. Accordingly, the unconstrained so-lution in (2.15) becomes H = Q 7 [ © r Q 0 + R ] a (2-16) SQ{JVx(k) + Tu(k-l) + &Ofi(k)-T(k)} SRiV(*) (2.17) S Q 0ft SQ S RQ J [ 0 (2.18) Chapter 2 24 where \ is the "left division" operator (used in M A T L A B ) . Many efficient methods exist for solv-ing equation (2.18) without running into the numerical difficulties involved in taking the inverse of the Hessian, as in (2.15). A controller using the solution in (2.18) was implemented on the M D M S in [McCourt & de Silva, 2003]. Unfortunately, the least squares approach outlined above only works for the unconstrained solu-tion. When constraints are involved, the length of the vector cannot always be set equal to zero, nor can the gradient of the cost function necessarily be set equal to zero as in equation (2.14). In this thesis active set methods are used to solve the constrained optimization problem. These methods typically involve taking the inverse of the Hessian. In view of this it is important to un-derstand what factors affect its condition number so that i l l posed problems can be avoided. If A, is an eigenvalue of A, then cAk wi l l have the eigenvalue cXf. Since the singular values are the square roots of the eigenvalues of A H A*, it can be seen why the M P C problem tends to be i l l conditioned. The Hessian in (2.16) contains the matrices © and Q. In equation (2.10) it is seen that the matrix 0 contains increasing powers of the state transition matrix, A of (2.4). Assuming that the plant is stable, the eigenvalues of A wi l l be on or within the unit circle and with increas-ing powers of A the corresponding eigenvalues (and therefore the singular values) wi l l become smaller compared to unity. So 0 is i l l conditioned and multiplying it by its transpose as in (2.16) wi l l only increase the condition number. The matrix 0 does make the Hessian i l l conditioned, but not nearly as much as Q. The formula-tion of D, in equation (2.12) shows that as the number of basis functions is increased, the elements in the lower-right corner of the matrix become very large, while the top-left corner contains an identity matrix. Consequently, multiplying the Hessian by Q. and its transpose wi l l greatly in-crease the condition number. In conclusion, the two factors that most significantly affect the condition of the Hessian are the prediction horizon and the order of the polynomial function used to describe the control input moves. So, a convenient way to maintain the condition of the Hessian is to either keep the pre-A H is the conjugate transpose Chapter 2 25 diction horizon low or use a low-order polynomial basis function. The latter is what is usually done in P F C , with M=2 or M=3 in equation (2.3) being typical [Maciejowski, 2002]. 2.7 Constrained Predictive Control Naturally, in any system there wi l l be physical limitations to the types and levels of inputs that can be applied and the nature of the resulting outputs. For the multi-module deployable manipu-lator system ( M D M S ) , which is the plant that is used in the present investigation, the input con-straints are the torque limits of the motors, and the output constraints are the joint motion limits. In the unconstrained controller, inputs are simply saturated when the constraint is violated and as a result the optimality with respect to the cost function is lost. If the constraints are formulated into the predictive controller, however, the controller wi l l be aware of the expected constraints and should be able to take corrective action before the constraints are actually met. Furthermore, the addition of constraints also allows the predictive controller to naturally handle such common controller problems as integrator windup. The constrained M P C problem may be expressed as the minimization of the cost function V(k) =[yVx(k) + Tu(k-\)-T(k)J Q[xHx(k) + ru(k-\)-T(k)] + mm ^(kf ¥r [yVx(k) + ru(k-\)-T(k)\ + fi(k)' H fi(k)^ (2.19a) subject to the constraints G/t(k)-t), y, = y0+vy-t-I, sm(e0+(D-t), 9, = 90+cv-t, (3.5) A i ^ ( * ) = KM P C.{7'(*)-¥*(*)-r«(*-l)} Chapter 3 38 In the constrained controller, the vectors T(k), x(k) and u(k-\) are the Np parameters of the opti-mization and in the mp-QP algorithm they form the parameter space 9?A/' that is partitioned in the offline optimization (see Appendix A) . The mp-QP algorithm computes the constrained optimal minimization of the controller cost function offline, and produces a look-up table for fast online implementation. Unfortunately, it was found that both the length of time needed to compute the offline solution, and the size of the look-up table, increased very fast with the size of the parame-ter space dl'"1'. In order to decrease the size of the parameter space and increase the computa-tional efficiency, a second-order Taylor series approximation of the model is formulated into the problem. We get Here r(/c) and r(k) are vectors of the current joint reference velocities and accelerations, re-spectively, and Ts is the sampling time of the controller. Equation (3.6) assumes that the current joint acceleration is constant throughout the prediction horizon. Since the window of time in which the controller looks into the future is small relative to the dynamics of the target, the ap-proximation given by equation (3.6) is adequate. When the joint velocities and accelerations are added to the problem, care must be taken when passing near, or through, singular configurations where the velocities and accelerations can be-come very large. Some examples of methods that deal with singularities involve reparameteriz-ing the desired path [Loyd, 1998], or adding damping factors to the singular values of the ma-nipulator Jacobian that are approaching zero [Nakamura, 1991]. Alternatively, Schreiber, et al. [1999] solved the singularity problem by constraint optimization. The controller developed in this thesis is a constrained optimal controller; therefore, it would be natural to adopt the constraint optimization solution to the singularity problem. There is particular advantage of the M D M S in this regard, as it has fewer singular configurations than a standard, all revolute joint manipulator. For the setup of the M D M S shown in Figure 3.1 there are no singularities in the operating space of the manipulator. The only singularity occurs when joints 1 and 3 occupy the same space, but the limits of the prismatic joint prevent the robot from ever reaching that configuration. r(k + i\k) = r(k) + r(k)iTj) + ±r(k)-{Tj)2 (3.6) Chapter 3 3.4 The Control System 39 The model of the manipulator has been formulated in the previous sections of the present chapter. The controller can be constructed now assuming that the expected joint-space position of the tar-get is known. As mentioned earlier, the expected joint-space location of the target has been cho-sen as the reference value. Typically, in robot motion control, a trajectory is generated to specify how the controller should move the manipulator between any two points. However, since the problem addressed in the present thesis is a robotic capturing task, the robot must somehow span the distance between some initial starting position and the desired position of the target before it is able to follow the reference point like a standard trajectory tracking problem. Some work has been done in the area of online trajectory planning for robot catching, for exam-ple, [Croft, et al , 1998] and [Park & Lee, 1992]. However, like for any online optimization prob-lem, in the controller developed in this thesis, simplifications must be made in order to achieve acceptable computation speeds. In [Croft, et al., 1998] the planner is used to direct the robot from its initial configuration to a minimum time interception point using an active prediction planning and execution (APPE) algorithm, then switching to fine motion tracking and grasping algorithms once interception is achieved. In [Park & Lee, 1992] the target moves in a straight line along a conveyor and the initial position of the end-effector is overtop of the conveyor, so the trajectory optimization is reduced to a one-dimensional problem. The controller developed in this thesis directly minimizes the joint motion differences between the reference target and the current state of the manipulator. However, it does this by trying to anticipate the motions of both the target and the manipulator. The controller is tuned to provide the desired tracking performance, and output constraints are used to control the speed at which the joints approach their targets. 3.5 Integration of Predictive Control As mentioned in Chapter 2, the cost function of the present predictive controller minimizes the predicted reference tracking errors and the corresponding control actions. The cost function and the constraints are formulated as Chapter 3 40 V(k) =\yx{k) + Tu(k-\)-T(k)~l Q [Vx(k) + ru(k-\)-T(k)] mmL(kf¥T [^x(k) + Tu(k-\)-T(k)] + /i(kf H ft(k) (3.7) s.t. Gn(k). Then the cost function becomes V{k) = ^ (k)' Y^(k) + ^ M{k)' H M{k) + ^ {k)rFM{k) (3.9) where: Y = 2[^ r - A ] 7 ' Q [ T r - A ] H = 2 n r ( © 7 Q 0 + R ) n F = 2[Y r -AfQ©a Chapter 3 i l Equation (3.9) is now in the form of the cost function used in the mp-QP solver in the Mul t i -Parametric Toolbox (see Appendix A) . The constraints in equation (2.20) also must be formu-lated in terms of the parameter vector, %{k). This gives Gft(k) -10 -20 I if 1 Time (s) Time (s) target - no predictions -predictions Figure 4.2 - Results from unconstrained controller showing the benefits of using predictions. The results in Figure 4.2 show how the predictive controller can be tuned to produce accurate tracking results. However, in achieving these results, the manipulator joints overshoot their target positions. The only way this could be corrected in the unconstrained controller was by decreasing the weights on the position tracking errors relative to the weights on the velocity tracking errors. Chapter 4 53 Unfortunately, when less importance was placed on the joint position errors the tracking perform-ance was found to suffer. When the saturation of the control input signal was removed the degree of overshoot was signifi-cantly reduced; however, very large control inputs were needed. The decrease in overshoot was due to the fact that the controller was making more accurate predictions. Without the inputs be-ing saturated the controller was able to apply the optimal solution that was calculated. So, when the controller is unaware of the abilities of the plant (i.e., its input and output constraints), the minimization of the plant error cannot be fully realized, and the performance degrades. 4.3 Constrained Predictive Control When constraints are included in the predictive controller, the computations involved can become very time consuming and the online optimizer may not converge to the optimal solution within the control period. For this reason, most constrained optimal predictive controllers for high-bandwidth processes have been limited to simulations. In this thesis, since both simulation and experimentation are carried out, multi-parametric programming techniques are used to improve the computational speed of the optimization. However, the calculation of the explicit offline so-lution to the constrained predictive controller can be very time consuming, and the solution must be re-computed every time an adjustment is made to the tuning parameters. So, as a means of comparison, and to show the full benefits of constrained predictive control, the optimal solution is first computed by solving the optimal control problem at each time step using the 'quadprog' QP solver from the Optimization Toolbox in M A T L A B . 4.3.1 QP Solution The benefits of adding constraints into the predictive control problem can be seen from the results presented in figures 4.3 and 4.4. The tuning parameters were all kept the same, but the ±10V constraints on the inputs were formulated into the optimization. When the controller is aware of the input constraints it makes a much more cautious approach towards the target. The input con-straints let the controller know how fast the M D M S can slow down when it gets to the target. Chapter 4 54 Figure 4.3 - Results of input constraint realization with unconstrained controller tuning parameters in joint space. Chapter 4 55 o 0.2 a. -0.2 -0.4 End-ef fec tor Pos i t ion 0.2 0.4 0.6 x -pos i t i on (m) 0.8 100 .2 «T 50 -] ro a End-ef fec tor Or ienta t ion T i m e (s) 1P - target • MDMS Figure 4.4 - Results of input constraint realization with unconstrained controller tuning parameters in task space. When input constraints are used, the controller makes more knowledgeable decisions, and the tracking error weights can be adjusted to take advantage of this knowledge. In figures 4.5 and 4.6 the velocity tracking weights were reduced until the controller gave a critically damped response. The results show that with the inclusion of constraints the manipulator is able to achieve a steady state tracking error equal to that of the unconstrained controller in Figure 4.2, and in the same interval of time, but now without overshooting the target position. Chapter 4 56 Figure 4.5 - Results from the re-tuned constrained controller with input constraints in joint space. Chapter 4 57 0.8 0.6 0.4 o 0.2 Q. -0.2 -0.4 End-ef fec tor Pos i t ion 0.2 0.4 0.6 x -pos i t i on (m) 0.8 100 o 50 1 S CD re 0 (A.3c) X){Wj +E£(k)-G/M (k)) = 0 (A.4d) where L [fi, 2.) = V (/c) - X1 (w + E% (k) - G/i (/c)) is the Lagrangian, with corresponding Lagrange multipliers, k, and-the subscript j denotes the row. By solving (A.3a) we get (ft', X ) = Rfi' (k) + FT$(k) + GTA' = 0 ft'{k) = -W1(¥Tit(k) + GTX) (A.4) where the X and G contain only the rows corresponding to the active set of constraints. For the inactive constraints, Xj = 0 in (A.4d), and for the rows in the active set, equation (A.4d) gives W + E$(k)-GS(k) = 0 ( A 5 ) Substituting (A.4) into (A.5) and solving for k we get, Appendix A 86 W + E^ (k) + G I T 1 (¥'%(k) + GTi) = 0 i = ( G H G R ( w + (E + GH'F7)5(Jfc)) i = -(GWiGT)~l(w + SZ>(k)) (A.6) where S - E + G H _ 1 F 7 . Equation (A.6) can now be substituted back into (A.4) to give, At the beginning of each control cycle, a new set of parameters is defined and equations (A.3) must be satisfied to determine the set of active constraints. Within a certain neighborhood of a given set of parameters, the active set of constraints wi l l remain constant, and an affine control law wi l l be defined throughout this region. The mp-QP algorithm determines (offline) the re-gions where the K K T conditions can be satisfied with the same set of active constraints. In doing N so, the parameter space, 91 '' , is partitioned into a total of Nr different regions. The regions are defined by the inequalities (A.3b) and (A.3c), which can be written in terms of the parameters, £;(£), using equations (A.6) and (A.7). As a result (A.3c) becomes, ft* (k) = IT' (G 7' (GH-'G 7')"' S - F 7 ' W / C ) + H - ' G 7 ' (GH-'G 7')"' W, (A.7) which is affine in the optimization parameters, %{k) e 91 0 (A.8) and equation (A.3b) becomes, Appendix A ; 87 The rows of equations (A.8) and (A.9) form the facets of an A^-dimensional convex polytope, within which the active constraints at the minimizer of the cost function remain unchanged. Each %(k) e SR^' wi l l belong to one of the N, polytopes, and the optimal solution is given by the corre-sponding affine control law. The mp-QP algorithm stores these Nr polytopes and their corre-sponding control laws into a look-up table. A.3 Offline Algorithm The offline mp-QP algorithm is used to partition the TVp-dimensjonal space into the TV, different regions. The algorithm developed by Tondel, et al. [2003a] is outlined here to show the main steps taken in generating the predictive controller look-up table. Algorithm A . l Let R be the regions explored. Let nR be the number of new regions found. 1) Start with an arbitrary, feasible point, 1/,2+/1+/2 + / 3 +m2(d2-±lJf (d2 + T/,2 +lxd2 cos f?3) jm3lx sinf?3 j m3 (lsd2 cos 6^ + y / v 2 j 2 1 " '3 m,/.. sin 6, 2 '"3*.v Ym 3 (lsd2 cos t93 + Y / V 2 ) + 7 3 - y w 3/ t sin #3 T w 3 / v 2 + / 3 (B.6) Appendix B 92 \lm2 (d2 - jl^ + lm^ +m3lx cos03J0, J 2 -mJsd2 sin#30,03 -\m3lsd2 sin0 30 3 2 C(q,q) = ~ { m i {^2 ~\h) + m2,^7 + \ m J s cos03}0,2 -mds cos0,0,03 -\mds cos03032 (B.7) jmJsd2 sin 030,2 + mJx cos 030,c/2 Viscous (F v ) and Coulomb (F s) friction terms were then added to give the complete model: The model parameters used in the design of the controller are found in [Wong, 2000]. The pa-rameters are: • mi = 2.3 kg • m2 = 0.75 kg • W5 = 3.05kg • /,. = 0.30 m • la — 0.22 m Equation (B.8) assumes that the friction in the manipulator can be described by only static and viscous friction terms at the joints. O f course, this is a considerable simplification, and since the linear predictive controller is applied directly to the manipulator, the friction terms do not directly affect the internal model of the controller. So, values for F s and F v are chosen so that input com-mands to the simulated robot and the experimental setup display similar results. M(q)q + C{q,q ) + F ssgn(?) + Fv? = T (B.8) APPENDIX C: Further Results in Satellite Capturing Using Predictive Control C . l Control Horizon M P C Results A control horizon form of the predictive satellite capturing controller was developed prior to the PFC version discussed in this thesis [McCourt & de Silva, 2003]. This unconstrained controller used the least squares solution as discussed in C H A P T E R 2: . The full nonlinear predictions of the target satellite's position, and orientation, were used to form the vector of future setpoints, T(k), which was then used in the controller shown in Figure C. 1. A prediction horizon of 50 and a control horizon of 20 were used and three simulated satellite captures were performed. In the first, a simulated target passed by the manipulator at 0.1 m/s, parallel to the platform, and with no rotational velocity. In the second, the center of gravity of the target remained stationary relative to the base of the manipulator, and the target rotated at a con-stant angular velocity of 0.087 rad/s (5°/s). In the third scenario, the target both translated and rotated as it passed by the manipulator. The results in Table C . l are similar to the results in Figure 4.2, i.e., predictions of the future target position improve the tracking performance of the manipulator. This result is presented graphi-cally in Figure C.2, where the joint space tracking results have been plotted for the capture of the spinning, and translating, satellite. 93 Appendix C 94 IS ^ M P C Au(k) 2 - 1 U(k) Linear Plant r z->I y(k) = x(k) Figure C.l - Block diagram of the unconstrained control horizon form of the predictive controller. Table C. l - Steady state tracking error results from the control horizon form of the predictive con-troller. Target Prediction? Average Steady State Tracking Errors 0/ error d2 error 63 error Translating only yes 0.02211° 0.000819 m 0.18336° no -2.08490° 0.000692 m 1.83530° Spinning only yes 0.22539° 0.002350 m 0.35425° no -1.12806° -0.003110 m 1.81234° Translating and Spinning yes -0.21157° -0.002330 m 0.57952° no -1.72780° -0.003540 m 0.98413° \ Appendix C 95 Without Predictions With Predictions 10 2. -10 -20 -30 -40 1 1 2 3 0.55 0.5 0.45 0.4 t ime (s) 2 3 4 t ime (s) -MDMS -Target t ime (s) Figure C.2 - MDMS joint-space tracking error response with, and without, predictions of the targets position. Appendix C C.2 More Experimental Satellite Capture Results 96 Experimental results from Chapter 5 demonstrated the implementation of the unconstrained and constrained predictive controllers on the prototype manipulator in our laboratory. Results from satellite targets with different angular and translatory speed have been presented in the following table, and figures to show the ability of the linear predictive controller to guide the Multi-module Deployable Manipulator System (MDMS) towards a variety of spinning targets. Table C.2 - Experimental results from various satellite captures Target details Tracking accuracy Case No. v, (m/s) vv (m/s) co (rad/s) Position Orientation Figure No. 1 0.005 -0.050 71/12 1.8±2.7mm 0.09°±0.14° D.3, D.4 2 0.000 -0.050 TC/24 l.l±0.8mm 0.15°±0.09° D.5, D.6 3 0.000 -0.100 0 1.7±0.9mm 0.29°±0.08° D.7, D.8 Figure C.3 - Joint space constrained predictive controller results for Case #1. Appendix C 98 End-Effector Postion 0.9 -j 0.8 , 1 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 x-position (m) End-Effector Orientation 100 Time (s) Constrained Unconstrained Target Figure C.4 - Task space constrained predictive controller results for Case #1. Appendix C 99 Figure C.5 - Joint space constrained predictive controller results for Case #2. End-Effector Position 0.9 "i 0.8 -0.7 0.6 E c 0 5 o 8 0.4 CL 0.3 0.2 0.1 0 -0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 x-position (m) End-Effector Orientation 100 Time (s) Constrained -Unconstrained Target Figure C.6 - Task space constrained predictive controller results for Case #2 Appendix C 101 Figure C.7 - Joint space constrained predictive controller results for Case #3. Appendix C 1 0 2 End-Effector Position 0.9 • , — • — 0.8 A 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 x-position (m) End-Effector Orientation 100 -T Time (s) Constrained Unconstrained Target Figure C .8 - Task space constrained predictive controller results for Case #3.