Int'l Conf. Modeling, Sim. and Vis. Methods MSV'16 3 Kinematics-Based Simulation and Animation of Articulated Rovers raversing Rough errain Mahmoud arokh Department of Computer Sence, San Diego State Universit San Diego, CA 92182-772, U.S.A. Abstract he paper proposes a simple and computationall effient method for the modeling and animation of highl articulated rovers traversing rough terrain. he method is based on the propagation of position and orientation veloties through wheels and various joints and linkages of the rover. hese velot equations are combined to form the Jacobian matri of the rover that relates the position and orientation of the rover to various active (actuated) and passive rover joint variables. A rearrangement of the Jacobian equation allows determining the actuation of suspension joints for balance control to avoid tipover. his is done through a pseudoinverse method which optimies a balance performance criterion. o illustrate the kinematics modeling and balance control concepts, the method is applied to a rover similar to the NASA s Sample Return Rover and simulation and animation results are presented. 1. Introduction Articulated rovers are being used in variet of applications, espeall in planetar eplorations. Rovers with active suspension mechanisms are capable of modifing their configurations b adjusting their suspension linkages and joints so as to change their center of mass, thus avoiding tipover while traversing rough and slopp terrain. Rovers with adjustable suspension sstem have been researched in recent ears. Iagnemma and Dubowsk [1] presented stabilit-based suspension control for a spefic rover using an essentiall geometric approach and performing a rather comple optimiation procedure. Other contributions in the area of rover kinematics include [2] and [3]. We have developed a comprehensive method for rover kinematics modeling [4] and included motion control in [5] In a subsequent paper [6], we presented an alternative and more effient method for kinematics modeling and balance control. he purpose of the present paper is to demonstrate the modeling and control using simulations and animation of a rover over uneven terrain. 2. Overview In this section we provide an overall view of our proposed simulation and animation environment which has been developed in Matlab/Simulink. he simulations consist of a number of modules as shown in Fig. 1. he trajector generation module provides either a time trajector for the desired rover position (, ) and heading, or a desired path. hese quantities are assumed to be available from a path planner. hese desired quantities are compared with their respective sensed values in a trajector following module that produces the desired forward speed and turn (aw) rate of the rover. he actuation or control module then receives the these speed and turn rate commands as well as the current sensed rover roll, pitch and suspension sstems joint angles to determine the actuation commands for the rover steering, wheel motors and active suspension sstem joints. hese commands are applied to the rover model which interacts with the terrain and produces the actual (sensed) rover quantities. Since trajector generation and trajector following have been covered in the literature, we will not discuss these modules in this paper. Animation consists of drawing the terrain, rover bod, suspension sstem, wheels and their interaction with the terrain. he animation module receives various sensed quantities from the rover simulation and incrementall moves and displas the motion in a smooth manner. 3. Kinematic Based Actuation and Control In this section we develop a kinematics model for and articulated rover with active suspension sstem (ARAS), which will be used to determine the actuation commands. Such a rover is a wheeled mobile robot consisting of a main bod connected to wheels via a set of linkages and joints that can be adjusted, some activel and some passivel for keeping the rover balanced. he active linkages and joints have actuators through which their values can be controlled, whereas passive ones change their values to compl with the terrain topolog.
4 Int'l Conf. Modeling, Sim. and Vis. Methods MSV'16 Fig. 1 he main simulation modules For modeling, we must determine the contributions of each wheel and suspension mechanism to the overall motion of the rover. We attach a number of frames starting from the wheel-terrain contact frame then going through the steering and suspension frames and finall to the rover reference frame. Since we are interested in the motion, we relate the translational and rotational veloties of the net frame in terms of the previous frame. Let ua [ a a a ] and ub [ b b b ] denote the position of the current and net frames, respectivel. Similarl, let a [ a a a ] and b [ b b b ] be the orientation of the current and net frames, respectivel, where, and are the rotation around, and ais, or roll, pitch and aw, respectivel. he 31 translation velot vector of the net frame b is dependent on the translational and rotational veloties of the current frame a plus an translational velot added to the frame b itself. his can be written as [7] u a a pb u ob u b Rb,a (1) where R b, a and pb are, respectivel, the rotation matri and position vector of the frame b relative to the frame a, and u ob is the translational velot added to the frame b. he latter is ero if the joint assoated with the frame b is not prismatic. he rotational velot of the net frame b is dependent on the rotational velot of the frame a plus an rotational velot added to the frame b itself, i.e. [7] ob b Rb,a a ob (2) We start at wheel i ( i 1,2,, n ) contact frame which has the translational and rotational veloties u [ ] and [ ], and perform the frame to frame velot propagation until we reach to the rover reference frame to obtain rover veloties u r [ r r r ] and r [ r r r ]. Let the joint variable vector that includes each wheel-terrain contact angle, steering angle, and various prismatic and revolute joint variables be denoted b the i 1 vector i. hen we will obtain an equation of the general form u r J r i u ; i 1,2,, n (3) i where J i is the Jacobian matri of the wheel i. Note that the wheel translational and rotational velot vectors u and include various slips. Equation (3) describes the contribution of individual wheel motion and the connecting joints to the rover bod motion. he net bod motion is the composite effect of all wheels and can be obtained b combining (3) into a single matri equation as I6 u c u r J c r I6 where the composite identit matri on the left is 6n 6, u c u c1 u c2 u cn and c c1 c2 cn are 3n1 vectors of composite wheel veloties at the contact points, and is the 1 vector of the joint variables which has both active (actuated) and passive joints. Note that in general some wheels share common suspension links and joints so that i n 1 i. he composite Jacobian matri of the rover J has a dimension of 6n (6n ). he composite equation (4) reflects the contribution of various position and angular rates to the overall motion of the rover. In order to control, we must determine commands to the wheels, steering and joints actuators. For this, we rearrange (4) into an equation of the form (4)
Int'l Conf. Modeling, Sim. and Vis. Methods MSV'16 5 A B q (5) where is the n 1 vector of unknown quantities to be determined, and q is the n q 1 vector of known quantities. he unknown vector consists of actuation signals such as active suspension joints, wheel roll rates, and un-measurable quantities such as appropriate slips. he known vector consists of desired quantities such as the desired forward rover velot d and heading d. he matrices A and B are obtained from the elements of J and the identit matrices I1,I2,, I6 in (4). After partitioning (4) into the form (15), the dimensions of A and B are 6n n and 6n nq. In general n nq, in which case we have an underdetermined sstem of equations. he etra parameters can be used to achieve an optimiation goal. For a rove with an active suspension sstem, the optimiation goal would be balanng the rover b adjusting its suspension joints and linkages so as to avoid tip over when traversing rough terrain. A possible optimiation function is analed is shown in Fig. 3. he rover has four wheels with each independentl actuated and rotation angles subscripted with a clockwise direction so that 1, 4 are for the left side and 2, 3 are for the right side. At either side of the rover, two legs are connected via an adjustable hip joint. In Fig. 3 the hip angles on the left and right sides are denoted as 2 1 and 2 2, respectivel. hese joints are actuated and used for balanng the rover. he two hips are connected to the bod via a differential which has an angle on the left side and on the right side. On a flat surface is ero but becomes non-ero when one side moves up or down with respect to the other side. he differential joint is passive (unactuated) and provides for the compliance with the terrain. he wheels are steerable with steering angles denoted b i. he wheel terrain contact angle i is the angle between the -aes of the i-th wheel ale frame Ai and contact coordinate frame as shown in Fig. 4. f a1 a2 a ˆ a (6) he first term is a tip over measure which reflects the degree to which the rover configuration deviates from the perfectl balanced configurations. he latter occurs when the rover in the second moves over a flat surface. he quantit a term is the vector of actuated suspension joints and ˆ a its nominal or desired values under normal operating conditions (e.g., operating over flat surface). Note that without the second term, minimiing f would result in a rover configuration that is maimall flat or spread out even when the rover moves over a flat surface. he weighting factors a1 and a2 place relative emphasis between achieving rover balanng and the desire to operate near the nominal configuration. We solve (5) and minimie (6) b using the null space of A, to get [8] Fig. 2 he JPL s sample return rover steerin adjustable height differentia steerin # # f / A B q k ( E A A ) (7) # where A is the pseudo-inverse of A, k is a scalar, E is n n identit matri, and the partial derivative are gradients of the performance function with respect to the active suspension joints, roll and pitch, respectivel. In the net section we spef the above quantities for our ARAS. he gradient can be computed numericall or analticall or numericall. 4. Modeling of an Active Suspension Rover he articulated rover with active suspension (ARAS) to be considered here is similar to the JPL Sample Return Rover shown in Fig.2. he schematic diagram of ARAS to be wheel 1 wheel 4 Fig. 3 Schematic diagram of the left side of ARAS A i c i c i c i Ai c i Fig. 4 Definition of contact angle In order to derive the kinematics equations, we must assign coordinates frames. Fig. 5 illustrates our choice of
6 Int'l Conf. Modeling, Sim. and Vis. Methods MSV'16 coordinate frames for the left side of the rover. he right side is assigned similar frames. In Fig. 5, R is the rover reference frame whose origin is located on the center of gravit of the rover, its -ais along the rover straight line forward motion, its -ais across the rover bod and its -ais represents the up and down motion. he differential frame D has a vertical (along -ais) offset denoted b k1and a horiontal distance of k2 from D. he distance from the differential to the hip, denoted b k 3, is half the width of the rover. We now introduce three more frames, all of which have origin at the wheel ale. he length of the legs from the hip to the wheel ale is k 4. he hip frames H 1,, H 4 for the four wheels are obtained from the differential frame b rotation and translation as shown with the Denavit-Hartenberg (D-H) parameters dh,d dh, adh and dh in able 1 and in Fig 5. Similarl the steering frames S1,, S4 and ale frames A1,, A 4 are defined in able 1 and Fig 5. We now use the basic frame to frame equations (1)-(2) and go through the frames sequentiall from wheel i terrain contact c i, wheel ale A i, steering S i, hip H i, differential D, and finall to the rover reference R. Equation (1)-(2) for the contact to the ale becomes u Ai Ai R R Ai, Ai, u where the rotation matri is evident from Fig. 4. R Ai, r c s 1 (8) s, as c S1-9 Fig. 6 Side (left figure) and top views of wheel 1 ABLE 1- D-H PARAMEERS FOR HE ARAS Frame dh d dh adh dh D k1 k2-9 H1 9 1 k3 k4 H2 9 2 -k3 k4 H3 9 3 -k3 k4 H4 9 4 k3 k4 S1 1 9 9 S2 2 9 9 S3 3 9 9 S4 4 9 9 A1 1 A2 2 A3 3 A4 4 Net we form wheel i ale to steering velot propagation as H1 S1 u Si RSi, Ai u Ai Ai Si RSi, Ai Ai i H1 S1 A1 A1 S1 (9) D k 2 k1 R k3 k4 k 4 H 4 c i s i where RSi,Ai s i c i. he net in the chain is 1 the hip frame, and we can write u Hi RHi,Si u Si Si Hi RHi,Si Si hi i (1) H 1 Fig. 5 Reference R, differential D, and hip H coordinate frames s( hi i ) c( hi i ) with RHi,Si c( hi i ) s( hi i ), 4 1, 1 1 i 1,2 3 2, and h i. Similarl the differential 1 i 3,4 frame, and rover reference frame veloties are obtained using (1)-(2) and able 1.
Int'l Conf. Modeling, Sim. and Vis. Methods MSV'16 7 Substituting recursivel (5) through (8) into (9) we obtain an equation of the form (3) where i i i i. Due to space limitation, the Jacobian matrices Ji and their elements are not given here but can be found in our technical report [9]. he elements of Ji are trigonometric functions of the joint variables i, i, i and. 5. Simulation and Animation Results In this section we present the results of balance control for the ARAS introduced in Section II.A.. he full Jacobian equation is not given here due to space limitations but is provided in [9]. For the ARAS, the vector in (5) is r r r r u c c (11) where r, r and r, r are the unknown rover veloties and attitude angles rates and hip rate vector. Note that u c rates i and side slip rate 1 2 is the actuated contains onl the wheel rolling i, and wheels are assumed to be in contact with the terrain so that i. In addition, c consist of onl wheel turn slip rates and tilt rate, with other components are removed due to the particular geometr of the rover. Finall; c is set to ero as contact angle rates are ver nois and their estimation is prone to large errors. hus is a 231 vector and A is a 6n 23 24 23 matri. Note that in general some rows of A are linearl dependent and thus rank(a) 24. he vector of the known quantities in (5) is q d d (12) where d and d are the desired (spefied) rover forward velot and heading (aw) rate, respectivel, and 1 2 3 4 is the steering angle rates vector. Note that since the aes of steering and wheel turn slip are coindent in this rover, the steering angles are indistinguishable from turn slip. In this case, a geometric approach is used to determine the steering angle rates using the desired forward velot d and d, and thus is a known quantit. hus the dimension of the known vector q is 6 1, and B has the dimension 24 6. We have developed a simulation and animation environment using Matlab/Simulink. he overall scheme is was described in Section 2. Fig 7 and Fig. 8 show the simulation environment where various quantities such as rover position and orientation, and various joint angle values are shown during the traversal. Various views of the rover can been observed. Fig. 7 he simulation environment Fig. 8 Side and front views
8 Int'l Conf. Modeling, Sim. and Vis. Methods MSV'16 We stud the behavior of the rover for three terrains. he rover speed is set at 1cm / s to ease the conversion between distance and time on the graphs. Case 1: Inclined errain he terrain and the trace of the rover wheels are shown in Fig. 9. he terrain is flat but has a 45 degree slope which could result in tipover without actuated suspension. he hip angles start at their nominal values, e.g. 2 1 2 2 9 degrees. he hip joint angles as given in Fig. 1 shows that the right joint has decreased to raise the right side but the left angle is increased to lower the left side. his has almost leveled the rover as is evident b the rover roll angle shown in Fig, 11 where the initial roll angle of about 38 degrees has been reduced to about 13 degrees. Case 2: Wav and Bump errain he terrain shown in Fig. 12 consists of a bump which is wav (sinusoidal) under the left wheels and smooth under the right wheels. he hip joint angles are depicted in Fig. 13. It is interesting to note that the right and left hip joint values also go through sinusoidal tpe changes but in the opposite directions to maintain a level and balanced rover bod. he rocker also shows sinusoidal behavior. he rover roll and pitch angles are seen in Fig. 14. he rover roll ehibits small changes due to the hip adjustments, whereas the rover pitch goes through relativel large variations due to the traversing up and then down the wav bump. Fig. 11 Rover bod roll and pitch angle profiles Fig. 12 Wav and bump terrain Fig. 9 Inclined terrain and traces of rover wheels Fig. 13 Hip joint angle trajectories Fig. 1 Hip joint angle trajectories Fig. 14 Rover roll and pitch trajectories
Int'l Conf. Modeling, Sim. and Vis. Methods MSV'16 9 Case 3: Inclined Ditch and Bump errain In this case, the terrain has a slope of 45 degrees with a bump under the left side wheels and a ditch under the right side wheels as shown in Fig. 15. he hip angles are adjusted accordingl to balance the rover as shown in Fig. 16. he rover ehibits a maimum roll angle of about 16 degrees as seen from Fig. 17; much smaller than the 45 degree terrain slope. Fig. 15 Inclined ditch and bump terrain Fig. 16 Hip joint angle trajectories Fig. 17 Rover roll and pitch trajectories 6. Conclusions A methodolog is presented for the kinematics modeling and control of articulated rovers for achieving rover balance when traversing rough and slopp terrain. he main feature of the work is the formulation of rover kinematics which uses simple velot propagation starting from wheel-terrain contact and going through various joints and linkages to finall reach the rover reference frame. his formulation makes the modeling and computer implementation ver effient through simple repeated function calls. Rover balance control is achieved through a pseudo-inverse method which optimies balance criterion. Simulation and animation package using Matlab have been developed that show the motion of the rover over rough terrain. References [1] Iagnemma, K. and S. Dubowski, raction Control of wheel mobile robots in rough terrain with applications to planetar rovers, Int. J. Robotics Res., vol. 23, No. 1-11, pp. 129-14, 23. [2] H. D. Naar, I. A. D. Nesnas, Re-usable kinematics models and algorithms for manipulators and vehicles, Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Sstems, San Diego, 27. [3] J. Balaram, Kinematic observers for articulated rovers,, Proc. IEEE Int. Conference on Robotics and Automation, pp. 2597-264, San Fransco, CA, 2 [4] M. arokh, M. and G. McDermott, Kinematics Modeling and Analsis of Articulated Rovers, IEEE rans. Robotics, vol. 21, No. 4, 539-553, 25. [5] G. McDermott, M. arokh, L. Mireles, Balance control of articulated rovers with active suspension sstems, Proc. 7th IFAC Int. Conf. on Robot Control, vol 8, Pt. 1, identifier: 1.3182/2696-3-I-291.121, Ital, Sept. 26. [6] M. arokh, H.D. Ho and A, Bouloubasis, Sstematic kinematics analsis and balance control of high mobilit rovers over roughterrain, J. Robotics and Autonomous Sstems, vol. 61, pp. 13-24, 213. [7] Craig, J. Introduction to Robotics, Mechanics and Control, pp.144-146, Pearson Prentice-Hall, 25. [8] Nakamura, N., Advanced Robotics- Redundanc and Optimiation, Chapt. 4, Addison and Wesle, 1991. [9] Mireles, L., G. McDermott and M. arokh, wo approaches to kinematics modeling of articulated rovers,http://www-rohan.sdsu.edu/~tarokh/ lab/publications.html.