Nonlinear Disturbance Decoupling for a Mobile Robotic Manipulator oer Uneen Terrain Joel Jimenez-Lozano Bill Goodwine Department of Aerospace and Mechanical Engineering Uniersity of Noe Dame Noe Dame IN 66 USA (e-mail: jjimene@nd.edu) Department of Aerospace and Mechanical Engineering Uniersity of Noe Dame Noe Dame IN 66 USA (e-mail: billgoodwine@nd.edu) Absact: This paper considers the nonlinear disturbance decoupling problem for a robotic manipulator that is mounted on a mobile platform. A mobile manipulation system offers a dual adantage of mobility offered by a mobile platform and deterity offered by the manipulator. In this work the acking and nonlinear disturbance decoupling problems are studied with particular focus on disturbances due to uneen terrain. We show that this system possesses the necessary geomeic sucture for complete disturbance decoupling between the outputs and disturbances. The disturbances are modeled as changes in the effect of graitational forces on the mobile manipulator due to its motion oer a uneen terrain. Simulation results illusate complete disturbance decoupling een in the presence of significant disturbances using the designed nonlinear conoller.. INTRODUCTION This paper presents results for complete nonlinear disturbance decoupling for a manipulator mounted on a mobile platform subjected to arying graitational forces due to uneen terrain. A mobile manipulators built from a robotic arm mounted on a wheeled mobile platform proides better capabilities for numerous tasks. A mobile manipulator combines the deterous manipulation capability offered by the manipulator and the motility proided by the mobile platform. Inestigation of their stability conol design simulation and eperimentation for different situations has been studied by many researchers including Chen and Zalzala 997 Wang and Kumar 993 Chung and Velinsky 998 Nikoobin and Rahimi 9 and others. Yamamoto and Yun 996 studied the effect of the dynamic interaction between the manipulator and the mobile platform and showed that the system was feedback linearizable under the appropriate nonlinear change of coordinates. The manipulator acks a desired ajectory in a fied reference frame. Their objectie was to compensate the dynamic interaction through a nonlinear feedback to improe the performance of the oerall system. A modular approach of this analysis was presented in Yamamoto and Yun 997 which includes a detailed proof of the functional dependence of some of the dynamic terms of the equations. In this work that methodology will be etended in studying a mobile manipulator in which we will include eternal force disturbances into the system. The final goal of the disturbance decoupling problem is to find a state feedback law such that the output is unaffected by the disturbance. Related work on disturbance decoupling hae been studied on robotic manipulators and mobile platforms in Nijmeijer 983 Danesh et al. Joshi and Desrochers 986 Zhu et al. 993 Gao 6 Papadopoulos and Paraskeopoulos 98 Esada and Malabre and others. In the following sections we present the dynamic equations of the mobile manipulator which are coupled. A state space representation of the equations is presented which etends the deriation in Yamamoto and Yun 996. A nonlinear feedback conoller is designed which includes disturbance decoupling. The calculation of disturbance forces due to the motion of the mobile platform oer an uneen terrain is presented and simulation results are presented which illusate the position of the mobile manipulator during motion to follow indiidual task ajectories for the platform and arm and other ariables. It is shown that the outputs are completely decoupled from the disturbances. The main conibution of this paper is applying the disturbance decoupling method to a problem with practical utility and with a leel of compleity similar to real-world problems.. MODELING EQUATIONS The equation of motion of the robotic manipulator subject to ehicle motion Yamamoto and Yun 996 997 can be etended to include eternal force disturbances and it is gien by M r (q r ) q r C r (q r q r ) C (q r q r q ) τ r R r (q r q ) q J r T (q r ) r J T (q r ) () where q r θ θ T denotes the Lagrangian coordinates of a R manipulator q denotes the Lagrangian coordinates of the mobile platform M r is the inertia Mai C r represents the Coriolis and cenifugal terms C denotes the Coriolis and
cenifugal terms caused by the angular motion of the platform τ r is the input torque/force for the manipulator R r is the inertia mai which represents the effect of the ehicle dynamics on the manipulator r and are eternal force disturbance ectors on the center of graity on each arm link and J r T (q r ) and J T (q r ) is the task space Jacobian mai of each arm link. Each term mai/ector is presented in the Appendi. The equation of motion of the mobile platform with a mounted manipulator Yamamoto and Yun 996 997 including eternal force disturbances is gien by M (q ) q C (q q ) C (q r q r q q ) E τ A T λ M (q r q ) q R (q r q ) q r E J T (q )F e () where q denotes the Lagrangian coordinates of the mobile platform and will be described in the net section M and C are the mass inertia and the elocity dependent terms of the platform respectiely M and C represent the inertial term and Coriolis and cenifugal terms due to the presence of the manipulator τ is the input torque/force to the ehicle E is a constant mai λ denotes the ector Lagrange multipliers corresponding to the kinematic consaints R represents the inertia mai which reflects the dynamic effect of the arm motion on the ehicle F e is an eternal force disturbance ector on the mobile platform through its center and J T (q ) is the moing space Jacobian mai for the mobile platform. Combining the elocity and inertia terms in Eqn. () and Eqn. () respectiely equations of motion of the wheeled mobile manipulator are simplified to M r (q r ) q r C r (q r q r q ) τ r R r (q r q ) q J T r (q r )F r e J T (q r ) (3) M (q r q ) q C (q r q q r q ) E τ R (q r q ) q r A T λ E J T (q )F e where C r C r C C C C and M M M.. Consaint Equations of the Mobile Platform The following notation will be used in the deriation of the consaint and dynamic equations as is illusated in Fig.. () For the mobile platform ( y ) are the coordinates of the point P which is the intersection of the ais of symmey with the driing wheel ais in the inertial frame b is the distance between the driing wheels and the ais of symmey r is the radius of each driing wheel θ r and θ l are the angular positions of the right and left driing wheel respectiely φ r(θ r θ l )/b c(θ r θ l ) is the heading angle of the mobile robot measured from w X- ais d is the distance from P to the center of mass of the platform m c is the mass of the platform without the driing wheels and I c is the moment of inertia of the platform without the driing wheels about a ertical ais through P. () For the manipulator P b ( b y b ) are the coordinates of the base of the manipulator in the frame Σ θ and θ are the joint angles of the manipulator l and l are the arm lengths respectiely m w is the mass of each driing wheel and I m is the moment of inertia of each wheel and the motor about the wheel diameter. The mobile platform has two co-aial wheels drien by motors. There are three consaints to which the platform is subjected one is that the platform must moe in the direction of the ais of w Σ w Y φ w X b Y r P e P r P X θ Σ d P b θ l r l Fig.. Geomey of the mobile platform and the mounted R manipulator. symmey and the other two are rolling consaints i.e. driing wheels do not slip. The consaint equations are gien in mai form as A(q ) q where q y θ r θ l T and A(q ) is gien by A(q ) sinφ cosφ. () cosφ sinφ cb cb. State Space Formulation of Motion Equations The dynamics of the wheeled mobile manipulator are goerned by Eqns. (3) and A(q ) q. Since the platform elocity is always in the null space of A(q ) Yamamoto and Yun 996 from A(q ) q it is possible to define a ector of generalized coordinates η(t) such that q S(q )η(t) where S(q ) is a full rank mai whose columns are in the null space of A(q ). Thus cbcosφ cbcosφ cbsinφ cbsinφ S(q ). Differentiating q substituting for q into the first equation in Eqns. (3) and multiply by S T. Following a similar procedure for q which is substituted into the second equation in Eqns. (3) results in the system of equations S T M S S T R R r S M r ξ P Q q η r S T M Ṡη S T C S T E τ C r R r Ṡη I τ r S T E J T D T r J r Fe D T. J D 3 Using the state ector q T q T r η T q T r T the system can be rewritten as
ẋ Sη q r τ P ξ P Q P D F() G() p () ω ω 3 P D P D 3 p () p 3 () where τ τ τ r T ω T and ω r T and ω 3 T. Hence the state space form is ẋ F() G()τ p ()ω p ()ω p 3 ()ω 3. () ω 3. FEEDBACK CONTROL AND DISTURBANCE DECOUPLING In this section subsection 3. deries the output equations and follows Yamamoto and Yun 996. Subsection 3. includes disturbance decoupling results not presented in their work. We look to conol the mobile platform in such a way that P r is brought to P e so the manipulator is brought into the preferred configuration. Thus we select the coordinates of P r in the inertial frame Σ w i.e. w P r w r w y r y cosφ sinφ sinφ cosφ l ly to be the other two components of the output equation. The output equations for conolling the mobile manipulator are gien by w r ( y θ r θ l ) y w y r ( y θ r θ l ) e (θ θ ). (6) y e (θ θ ) h() The objectie of selecting these outputs is that the system is nonholonomic and it is not input state linearizable it is input-output linearizable if a proper set of output equations are chosen. In addition these set of outputs made the system proper for disturbance decoupling as shown net. 3. Output Equations The desired task ajectory for the endpoint of the manipulator P e in the frame Σ w is gien by w w P e (t) e (t) w. y e (t) The mobile manipulator shown in Fig. has four inputs two from the R manipulator and two from the mobile platform. We may hae up to four output ariables to be conolled. First we select the output ariables of the manipulator to be P e which represents the actual location of the end point of the manipulator. The coordinates of P e with respect to the platform coordinate frame Σ are gien by P e e y e l cosθ l cos(θ θ ) b l sinθ l sin(θ θ ) y b where the points P e and w P e are related by w P e w P R cosφ sinφ φ P e e. y sinφ cosφ y e The output ariables for conolling the mobile platform are chosen net. The objectie of the platform moement is to bring the manipulator into a preferred configuration. For this purpose we pick the configuration with the maimum manipulability measure as the preferred configuration of the manipulator. The manipulability measure can be regarded as a distance measure of the manipulator configuration from singular ones at which the manipulability becomes zero. At or near a singular configuration the endpoint of the manipulator may not easily moe in certain directions. The effort of maimizing manipulability measure leads to keeping the manipulator configuration away from singularity. The manipulability measure for nonredundant manipulators w l l sinθ Yoshikawa 99 and is maimized for θ π/ and arbiary θ. The endpoint of the manipulator at the preferred configuration is denoted by P r called the reference point. The coordinates of P r in Σ are gien by P r r y r l l b y b l. ly 3. Feedback Input-Output Linearization with Disturbance Decoupling We hae presented the dynamics of the mobile manipulator in the state space form Eqn. () and the output equation Eqn. (6). The ector field is modeled through the p()s. To achiee inputoutput linearization a nonlinear feedback has to be employed. To simplify state Eqn. () we applied the following feedback τ Q (Pu ξ ) (7) which simplifies the state equation as Sη ẋ q r u ω I P D f () g p () ω (8) ω 3 P D P D 3 p () p 3 () y h(). If the disturbances ω are aailable for measurements one can use a conol u α() β() γ ()ω γ ()ω γ 3 ()ω 3 Isidori. Then decoupling the output from the disturbance it is possible. The relatie degree of the system is r that is the number of differentiations of each component of the outputs until the input eplicitly appears in the deriatie ÿ. Following the analysis of Isidori the conol law soling the problem of decoupling y is gien by
α() L f h() L g L f h() Φ β() L g L f h() Φ Φ η q r γ () L p L f h() L g L f h() P D γ () L p L f h() L g L f h() P D γ 3 () L p 3 L f h() L g L f h() P D 3. So the nonlinear feedback is gien by ) u Φ ( Φ ΦP η qr (D ω D ω D 3 ω 3 ). (9) The mai Φ is presented in the Appendi. Substituting this nonlinear feedback Eqn. (9) into Eqn. (8) we obtain a linear and decoupled input-output relationship ÿ ÿ ÿ ÿ3 ÿ 3. The input-output relationship is decoupled because each component of the reference input i conols one and only one component of the output y i. To complete the conoller design it is necessary to stabilize each of the aboe four subsystem with another constant feedback. Therefore the entire conoller for the mobile manipulator consists of nonlinear feedback Eqn. (7) and Eqn. (9) followed by a linear feedback. We hae used a PD computed-torque conol law. We look for a desired ajectory y d which gies ÿ ÿ d K ė K p e with the acking error defined as e y y d. For our simulations K and K p 6 were selected. Our algorithm requires the calculation of mai operations i.e. mai inerse. During the simulation we acked the condition number of the maices in order to maintain stability. We used alues of K and K p that are well behaed and asymptotically decay to a constant alue once the acking errors are diminished.. DISTURBANCE FORCES We are interested in disturbance forces that are position dependent. In this case we hae assumed that the disturbances are related to changes in the graitational forces on the system due to the motion of the mobile manipulator oer a uneen terrain. The uneen terrain is modeled as a surface function U(y). The surface must be known or the robot must be equipped with a sensor that can determine the orientation of the graity ector. The form of the terrain for the simulations in this paper is illusated in Fig. 3 and the ajectory used for the simulations projected onto the -y plane is indicated by the solid line. For clarity of presentation the robot is modeled as moing along a flat surface but subjected to a force field that would result from the uneen terrain. A unit normal can be calculated at each point on the surface (or sensed if the robot is there) using n U/ U n î n y ĵ n zˆk. This normal ector is used to project the force due to graity onto the XY -plane. The graitational forces are gien by W cart m w gˆk for the cart W link m gˆk and W link m gˆk for each link respectiely. The forces are obtained by using Upper iew Side iew r z ( ( ( y y n W cart n n W link n n W link n n y W cart ) ) ) n () n () n. () Normal plane Fig.. Modeling of the disturbance forces.. z -. - y 8 n Fig. 3. Disturbance forces on the mobile platform and the uneen terrain. Surface function U. sin(.) sin(.y).. SIMULATIONS This section presents simulation results illusating the effectieness of the conoller. In the simulation indiidual task ajectories for the mobile platform and arm are inestigated. The mobile platform is initially placed at the origin facing toward the positie w X-ais of the inertial frame. The initial head angle is zero φ(). Platform and manipulator parameter alues are gien in Table we hae used the alues used in Yamamoto and Yun 996. The entire system is assumed to be stationary at t. The initial alues are ( y θ r θ l θ θ θ r θ l θ θ ) (..7
Table. Parameters alues used for the simulation Parameters Values Units r.7 m b.7 m l. m l. m m kg m kg m c 9 kg m w kg I c 6.69 kg m I m.3 kg m I w. kg m d m 7 ). Indiidual task ajectories for mobile platform and arm are w e () 3 r (t) t w P e (t) w y r (t) e (t) y e () 3 t y e (t) m e () ( π ) m y e ().7sin t where ( e () y e ()) () and ( m e () m y e ()) (..). The location of the arm base on the mobile platform are gien by b.m and y b.m. The cart geomey and its center () are shown in Fig. the saight solid line represents the mobile platform ajectory and the sinusoidal solid line represents the ajectory of the endpoint of the manipulator. Platform and arm positions are shown at different times the total period of time for the simulation was 9 seconds. The ariations of the joint angles of the manipulator during time are shown in Fig.. The ariation of the heading angle of the platform during the simulation is shown in Fig. 6. The acking errors are shown in Fig. 7. We hae estimated the acking error as the difference of the obtained ajectory to the desired ajectory as e i (t) y i (t) y di (t) for i... Initially there are oscillations in the acking error but later are reduced to ery low alues as epected. The outputs are completely decoupled from the disturbance forces; hence the outputs do not change with the disturbances. The effect of force disturbance can be obsered in the torques required during the motion of the system. The disturbance forces has been implemented assuming a surface U. sin(.) cos(.y) and forces were calculated using the methodology discussed in Section. The disturbance force components on the mobile platform during time are shown in Fig. 8. The computed torques for conol are shown in Fig. 9. The disturbance force components on the arm links are shown in Fig.. The computed torques for conol are shown in Fig.. It can be obsered that the disturbances are satisfactorily managed by the linear conol applied to the linear inputoutput relationship. 6. CONCLUSIONS We hae presented the solution to the disturbance decoupling problem for a system with a manipulator mounted on a mobile w Y m 8 6 w Y m 3 w X m 6.. 3. 3.. (a) Simulation 6 7 8 9 w X m (b) Close-up iew Fig.. Motion of the mobile platform and arm during indiidual task ajectories. Solid saight line linear task ajectory; Solid sinusoidal line tip of the arm; P ; dashed square; mobile platform position. θ deg 3 θ θ 3 6 7 8 9 t sec Fig.. Joint angles of the manipulator in time. φ deg 3 3 3 6 7 8 9 tsec Fig. 6. Heading angle of the mobile platform in time. θ deg
e e e 3 e m.3.....3...6..3.....3 e e 3 e....7 3 6 7 8 9 t sec Fig. 7. Tracking errors. y N r 3 6 7 8 9 t sec e y Fig. 8. Disturbance forces on the platform. τ τ N m 3 6 7 8 9 t sec Fig. 9. Computed platform torques for the platform. N y y r r 6 6 y r r y 3 6 7 8 9 t sec τ Fig.. Disturbance forces on the arm links. platform. The efficacy of the approach is illusated by imposing a force field on the system that would result from the mobile platform aersing uneen terrain. Future work will inole incorporating recent results of the authors related to the conol of mechanical systems and the notion of dynamic singularities Goodwine and Nightingale in order to etend these results to more complicated systems. Also a eperimental study τ τ r τ N m rτ rτ 3 6 7 8 9 t sec Fig.. Computed arm joint torques. on a real mobile manipulator system will be made to implement our algorithm and enforce our results. ACKNOWLEDGEMENTS This material is based upon work supported by the U.S. Army TACOM Life Cycle Command under Conact No. W6HZV- 8-C-36 through a subconact with Mississippi State Uniersity and was performed for the Simulation Based Reliability and Safety (SimBRS) research program. REFERENCES D. P. Papadopoulos and P. N. Paraskeopoulos. Decoupling Techniques applied to the design of a conoller for an unregulated synchronous machine. General Transmission and Disibution IEE Proceedings C 3(6):77 8 98. H. Nijmeijer and A. Van Der Schaft. The disturbance decoupling problem for nonlinear conol systems. IEEE Transactions on Automatic Conol AC-8():6 63 983. M. B. Esada and M. Malabre. Necessary and sufficient conditions for disturbance decoupling with stability using PID conol laws. IEEE Transactions on Automatic Conol (6):3 3. Z. Gao. Actie disturbance rejection conol: A paradigm shift in feedback conol system design. Proceedings of the American Conol Conference -6 June:399 6. A. Nikoobin and H. N. Rahimi. Analyzing the wheeled mobile manipulators with considering the kinematics and dynamics of the wheels. International Journal of Recent Trends in Engineering ():9 9 9. H. A. Zhu C. L. Teo G. S Hong and A. N. Poo. Motion conol of robotic manipulators with disturbance decoupling. Second IEEE Conol applications :397 993. M. W. Chen and A. M. S. Zalzala. Dynamic modeling and genetic based ajectory generation for non-holonomic mobile manipulators. Conol Engineering Practice ():39 8 997. C. Wang and V. Kumar. Velocity conol of mobile manipulators. Proceedings IEEE Int. Robotics and Automation :73 78 993. J. H. Chung and S. A. Velinsky. Modeling and conol of a mobile manipulator. Robotica 6:67 63 998. Y. Yamamoto and X. Yun. Effect of the dynamic interaction on coordinated conol of mobile manipulators. IEEE Transactions on robotics and automation ():86 8 996.
Y. Yamamoto and X. Yun. A modular approach to dynamic modeling of a class of mobile manipulators. International Journal of Robotics and Automation (): 8 997. Mohammad Danesh Farid Sheikholeslam and Mehdi Keshmiri. Eternal force disturbance rejection in robotic arms: An adaptie approach. IEICE Trans. Fundamentals E88- A(): 3. Jagdish Joshi and Alan A. Desrochers. Modeling and conol of a mobile robot subject to disturbances. IEEE International Conference on Robotics and Automation :8 3 986. Alberto Isidori. Nonlinear Conol Systems. Springer. T. Yoshikawa. Foundations of Robotics: Analysis and conol. MIT Press 99. Bill Goodwine and Jason Nightingale. The Effect of Dynamic Singularities on Robotic Conol and Design. Proceedings of the IEEE International Conference on Robotics and Automation. APPENDIX Detailed epressions for all of the terms contained in the equations of motion for the system. q q q q 3 q T y θ r θ l T q r r q r q T θ θ T M r 3 m l 3 m l m l cosθ 3 m l m l cosθ 3 m l m l cosθ 3 l m C r ml θ sinθ m l θ θ sinθ m l θ sinθ C (i) m n n Th T T h j k hma(ik) r J h q i q j r q j r q k q k m n n Th T T h j k hi r J h q i q j q j q k q k (i R j) n Tk T T r k ki r J k q i i n j m q j T i T A A...A i i i...n cosθ sinθ l cosθ A sinθ cosθ l sinθ cosφ sinφ (l /)sinθ J J sinφ cosφ r (l /)cosθ cosθ sinθ l cosθ A sinθ cosθ l sinθ cosφ sinφ sinφ cosφ y T l sinθ J (l /)sin(θ θ ) (l /)sin(θ θ ) l cosθ (l /)cos(θ θ ) (l /)cos(θ θ ) (i M j) n Tk T T k k J k q i i j m q j (i R j) n Tk T T k k j J k q i r i m j n q j 3 m l m l J m l m 3 m l m l J m l m m m c cd sinφ m c cd sinφ M m m c cd cosφ m c cd cosφ m c cd sinφ m c cd cosφ Ic I w Ic m c cd sinφ m c cd cosφ Ic Ic I w m c d φ cosφ C m c d φ sinφ E C (i) n m n Th T T h j k h j J h q i r q j q k n n n Th T T h j k hma( jk) J h q i r q j r q k Φ Φ Φ Φ Φ Φ 33 Φ 3 Φ 3 Φ r q j q k r q j r q k Φ (cb l y c)cosφ l sinφ Φ (cb l y c)cosφ l sinφ Φ (cb l y c)sinφ l cosφ Φ (cb l y c)sinφ l cosφ Φ 33 l sinθ l sin(θ θ ) Φ 3 l sin(θ θ ) Φ 3 l cosθ l cos(θ θ ) Φ l cos(θ θ ).