The Control of a Mobile Robot with Dual Wheel Transmission. TR-CIM October 2012

Transcription

1 The Control of a Mobile Robot with Dual Wheel Transmission TR-CIM-1-25 October 212 Master of Engineering Project Report Yann Goulet Garneau Supervisors: Jorge Angeles and Alessio Salerno Department of Mechanical Engineering & Centre for Intelligent Machines, McGill University, Montreal, Canada

2 Abstract This report describes the kinematics, dynamics, control and mechatronics implementation of a mobile robot with dual wheel transmission The robot comprises four wheels, two of which are coupled to two motors via a transmission which can pivot about the steering axis The prototype structure is made of LEGO parts and custom stainless steel axles The kinematics of the robot is described in terms of the twist of the 12 moving assemblies of the robot Inertial and mass parameters of the moving assemblies are evaluated using a 3D CAD model in SolidWorks The equations of motion of the system are derived in terms of the acceleration of the two actuated wheels and a configuration parameter of the robot in Maple The state-space form of the system is used along with pole placement to define a controller for the configurationdependent robot model Simulation runs under Simulink are performed to validate the controller The mechatronics implementation is done using the Arduino platform Finally, recommendations for future work are proposed 2

3 1 Introduction Wheeled mobile robots (WMR) are increasingly used in the manufacturing and distribution industry The dual wheel transmission (DWT) is a design that aims to increase the mobility and manoeuvrability of WMR in constrained environments The DWT couples two motors with corresponding wheels via a composite gear train which can pivot about the steering axis Two epicyclic gear trains are used to transfer the motion of each motor to its corresponding wheel The DWT was patented in 25 by Angeles [1] A sketch of the original DWT design is shown in Fig 1a (a) Figure 1: (a) The original DWT design sketch (b) (b) The robot prototype The symmetric design of the DWT allows equal loading on both motors, uniform wear of gears and tires, unlimited rotation of the actuated-wheel axis around the steering axis and eliminates wire entanglement that would occur with the motors connected directly to the wheels The robot described in this report carries two identical motors mounted onto a chassis with one DWT The robot motion can be associated with that of an articulated vehicle or a two-wheel robot with a trailer The robot prototype is made of standard LEGO parts with 3

4 custom stainless steel shafts A 3D model of the robot was developed with SolidWorks It is shown in Fig 1b In order to stiffen the original design, bevel gears were used on the LEGO prototype instead of universal joints This project is part of ongoing R&D effort; it follows preliminary work by Husseini [2], Pavlov [3] and Mak [4] The objectives of this project are to develop the robot kinematics and dynamic models, define a suitable control algorithm and implement it on the robot prototype The report includes sections on the mathematical modelling, control and mechatronics implementation of the robot, followed with conclusions and recommendations for future research In the mathematical modelling section, a description of the structure of the robot and its 12 moving rigid assemblies is first included The kinematics equations of the robot are then derived, followed by a description of the method used to derive the dynamics equations of the system In the control section, the dynamics equations are transformed into statespace form, linearized, and then used along with pole placement to find a suitable feedback control for the robot Finally, the mechatronics implementation section gives a description of the different components used to drive the system and control its behaviour 2 Mathematical Modeling We set out to define a control algorithm for the robot based on the dynamics of the system The derivation of the mathematical model describing the kinematics and dynamics of the system is based on the study of robotic mechanical systems and rolling robots by Angeles [5] Our approach will be one of multi-body dynamics; for this reason, we distinguish 12 rigidbody assemblies composing the robotic mechanical system The moving assemblies are shown in Fig 2 We label the rigid body assemblies with numbers as indicated below: 1 Front left wheel assembly 2 Front right wheel assembly 3 Rear left wheel assembly 4 Rear right wheel assembly 4

5 Figure 2: The 12 moving assemblies of the robot Motor 1 Motor (a) First epicyclic gear train (b) Second epicyclic gear train Figure 3: The transmission of the robot 5

6 5 Planet-carrier 6 Chassis 7 Lower left gear assembly 8 Lower right gear assembly 9 Inner sun gear assembly 1 Outer sun gear assembly 11 Upper right gear assembly 12 Upper left gear assembly The DWT comprises two epicyclic gear mechanisms coupled by the planet-carrier The first epicyclic gear train consists of parts 1, 7, 9 and 11 and is connected to motor 1 as shown in Fig 3a The second epicyclic gear train consists of parts 2, 8, 1 and 12 and is connected to motor 2 as shown in Fig 3b 21 Kinematics In deriving the mathematical model, we assume that the robot, except for its wheels, undergoes planar motion on a horizontal surface All the rigid assemblies except the robot s four wheel assemblies undergo planar motion We also assume that the wheels are always in contact with the ground, without slipping or slidding, and that internal dissipation such as dry and viscous friction is negligible [6] The basic variables required to describe the kinematics of the robot are shown in Fig 4 Let {O, X, Y, Z} be the inertial frame F attached to the ground, with the Z-axis vertical and with point O attached to the ground surface We define θ 1 and θ 2 as the angular displacements of the two front wheels, which are actuated It is assumed that having both θ 1 and θ 2 positive makes the planet-carrier, and hence, the axles of the wheels go forward, as indicated with v 5 in Fig 4 We also define φ 1 and φ 2 as the angular speeds of motors 1 and 2, which are provided with encoders to measure there speeds It is important to note that the angular speed of the motors and the angular speed of the wheels are not simply related to the gear ratios of the two epicyclic gear trains Indeed, the upper left and right gear assemblies (11, 12) rotate around the inner and outer sun gear assemblies (9, 1) The equations relating the angular speed of the two actuated wheels to the angular speed of the motors are derived later in the report 6

7 Y θ 3 θ 1 ω v 5 5 α l 5 θ 5 y 5 ω 6 v 6 θ 6 θ 2 y 6 l 6 r θ 4 l x 6 X Figure 4: Sketch for the kinematics of the robot x 5 We also define the actuated joint variables grouped into vector θ a, with their timederivatives being the actuated joint rates, grouped into vector, θ a, as: θ a [ ] T [ ] T θ 1 θ 2, θa θ1 θ2 (1) For the derivation of the kinematic equations of the robot, the methodology of the Natural Orthogonal Compliment (NOC) is used [5] The methodology exploits our ability to express the kinematic constraints on the robot as a linear homogenous system of equations in the vector array of body twists The twist of the ith rigid body t i is given by: ] t i [ vi ω i, (2) where v i is the absolute velocity vector of frame F i, attached to the ith rigid body, and ω i is the absolute angular velocity vector of the ith body expressed in the inertial reference frame Given the nonholonomy of the system at hand, we cannot obtain v i and ω i in terms of only θ a We need a third variable to define the twist t i of the robot We use angle α, defined 7

8 as that between the midplane of the the two front wheels and the midplane of the two rear wheels, as per Fig 4, ie, α = θ 5 θ 6 (3) Next, the twists t i are expressed as linear transformations of θ a : t i = T i (α) θ a, (4) where T i (α) is the 6 2 twist-shaping matrix of the ith rigid body, which is the NOC of the matrix K of kinematic constraints [5], that need not be explicitly calculated in this analysis Note here that only the twists of the chassis, upper left gear, upper right gear, rear left wheel and rear right wheel assemblies (3, 4, 6, 11, 12) are dependent on both angle α and θ a The twists of the planet-carrier, left wheel, right wheel, lower left gear, lower right gear, inner sun gear, outer sun gear assemblies (1, 2, 5, 7, 8, 9, 1) can be expressed as functions of θ a alone We now derive the absolute velocity v i and absolute angular velocity ω i for each rigid body as a function of α, θ1 and θ 2 Combining those two vectors yields the twist t i of the ith rigid body which can then be used to find the twist shaping matrices T i (α) of the same rigid body This task was achieved with the aid of Maple Planet-carrier (5) The variables describing the kinematics of the planet-carrier are shown in Fig 5 Let {O 5, X 5, Y 5, Z 5 } be the frame F 5 attached to the planet-carrier, with O 5 located at the intersection of the midplane of the two front wheels and the axis of rotation of the same wheels Furthermore, let Q 5 be the 3 3 orthogonal matrix that rotates frame F 5 with respect to the inertial frame F attached to the ground around the Z axis through an angle θ 5 : cos θ 5 sin θ 5 Q 5 = sin θ 5 cos θ 5 (5) 1 8

9 Y j 5 Y 5 θ 1 l 5 X 5 i 5 y 5 ω 5 O 5 v 5 θ5 θ 2 r X Figure 5: Sketch for the kinematics of the planet-carrier x 5 The position vector o 5 of the origin O 5 of the frame F 5 is given by: o 5 = x 5 i + y 5 j + rk (6) The absolute velocity v 5 and absolute angular velocity ω 5 of the planet-carrier, expressed in frame F 5, are given by: v 5 = r 2 ( θ 1 + θ 2 ) i 5, (7) ω 5 = r l 5 ( θ 2 θ 1 ) k 5 (8) The position vector of the planet-carrier, [x 5 y 5 θ 5 ] T, can be derived from the relations θ 5 = ω 5, ẋ 5 = v 5 cos θ 5, ẏ 5 = v 5 sin θ 5 (9) 9

10 Chassis (6) The variables describing the kinematics of the chassis are shown in Fig 6 Y y 5 y 6 j 6 Y 6 j 5 Y 5 ω 6 O 6 θ 1 v 6 θ 6 ω 5 O 5 v 5 α X 5 i 5 θ5 θ 2 r X 6 i 6 l x 6 X Figure 6: Sketch for the kinematics of the chassis x 5 Let {O 6, X 6, Y 6, Z 6 } be the frame F 6 attached to the planet-carrier, with origin O 6 located at the intersection of the midplane of the two rear wheels and the axis of rotation of the two same wheels Furthermore, let Q 6 be the 3 3 orthogonal matrix that rotates frame F 6 with respect to the frame F 5 around the Z 5 axis through an angle α: cos α sin α Q 6 = sin α cos α (1) 1 The position vector o 6 of the origin O 6 of frame F 6 in frame F 5 is given by: o 6 = l cos α i 5 + l sin α j 5 + k 5, (11) 1

11 or, in the inertial frame F: o 6 = (x 5 l cos θ 6 ) i + (y 5 l sin θ 6 ) j + r k, with θ 6 = θ 5 α (12) The absolute velocity v 6 and absolute angular velocity ω 6 of the chassis, expressed in frame F 6, are given by: v 6 = v 5 cos α i 6 = r 2 cos α( θ 1 + θ 2 ) i 6, (13) ω 6 = 1 l v 5 sin α k 6 = r 2l sin α( θ 1 + θ 2 ) k 6 (14) The position vector of the chassis [x 6 y 6 θ 6 ] T can be derived from the relations: θ 6 = ω 6, x 6 = x 5 l cos θ 6, y 6 = y 5 l sin θ 6, with θ 6 = θ 5 α (15) First epicyclic gear train - Front left wheel assembly (1) The variables describing the kinematics of the front left wheel assembly are shown in Fig 7 Y X 1 i 1 j 5 j 1 Y 5 Y 1 j' 1 Y' 1 θ 1 ω 1 O' 1 O1 v 1 l 5 ω 5 v 5 X 5 i 5 O 5 θ 2 r X Figure 7: Sketch for the kinematics of the front left wheel assembly 11

12 Let {O 1, X 1, Y 1, Z 1 } be the frame F 1 attached to the planet-carrier, with origin O 1 located at the intersection of the midplane of the left front wheel and the axis of rotation of the same wheel The position vector o 1 of O 1 in frame F 5 is given by: o 1 = i 5 + l 5 2 j 5 + k 5 (16) Furthermore, let {O 1, X 1, Y 1, Z 1} be the frame F 1 attached to the left front wheel, with origin O 1 coinciding with O 1 Let Q 1 be the 3 3 orthogonal matrix that rotates frame F 1 with respect to the frame F 1 around the Y 1 axis through an angle θ 1 : cos θ 1 sin θ 1 Q 1 = 1 (17) sin θ 1 cos θ 1 The absolute velocity v 1 and absolute angular velocity ω 1 of the left front wheel assembly, expressed in frame F 1, are given by: v 1 = r θ 1 i 1, (18) ω 1 = θ 1 j 1 + ω 5 k 1 = θ 1 j 1 + r l 5 ( θ 2 θ 1 ) k 1 (19) First epicyclic gear train - Lower left gear assembly (7) The variables describing the kinematics of the lower left gear assembly are shown in Fig 8 Let {O 7, X 7, Y 7, Z 7 } be the frame F 7 attached to the planet-carrier, with origin O 7 located at the intersection of the axis of rotation the lower left gear and its center of mass The position vector o 7 of frame F 7 in frame F 5 is given by: o 7 = i 5 + l 7 j 5 + (h 7 r) k 5, (2) where h 7 is the height of the center of mass of the lower left gear assembly Furthermore, let {O 7, X 7, Y 7, Z 7} be the frame F 7 attached to the lower left gear, with origin O 7 coinciding with O 7 Let Q 7 be the 3 3 orthogonal matrix that rotates frame F 7 with respect to frame 12

13 Y j' 7 j j 7 5 Y Y 7 5 Y' 7 i' 7 X' 7 X 7 i 7 X 5 θ' θ 7 θ 7 1 ω 7 v 7 θ 5 i 5 y 7 y 5 l 7 O 7 v 5 ω 5 O 5 θ 2 l 5 r x 7 X Figure 8: Sketch for the kinematics of the lower left gear assembly x 5 F 7 around the Z 7 axis through an angle θ 7: cos θ 7 sin θ 7 Q 7 = sin θ 7 cos θ 7 (21) 1 with θ 7 = θ 7 θ 5, ω 7 = ω 7 ω 5 (22) Let N 1,7 denote the ratio of the number N 7 of teeth of gear 7 to the number N 1 of teeth of gear 1 This ratio is also the ratio of the angular velocity of the left wheel θ 1 to the angular velocity of the lower left gear assembly ω 7, as observed in frame F 7, which is N 1,7 = θ 1 (23) ω 7 The lower left gear angular velocity ω 7, observed in frame F 7, is then ω 7 = 1 N 1,7 θ1 (24) 13

14 The absolute angular velocity of the lower left gear assembly ω 7 expressed in frame F 7 is given by: ω 7 = (ω 5 + ω 7) k 7 = [ r ( l θ 2 θ 1 ) 1 ] θ1 5 N 1,7 k 7, (25) The absolute velocity of the lower left gear assembly v 7 expressed in frame F 7 is given by: v 7 = (v 5 l 7 ω 5 ) i 7 = [ r 2 ( θ 1 + θ 2 ) rl ] 7 ( l θ 2 θ 1 ) 5 i 7, (26) First epicyclic gear train - Inner sun gear assembly (9) The variables describing the kinematics of the inner sun gear assembly are shown in Fig 9 Y i' 9 y 5 y 9 j 6 Y 6 j' 9 j 9 j 5 Y Y 5 9 Y' 9 θ 1 ω 9 ω 5 O 9 O' 9 O 5 v 5 X' 9 θ' 9 θ'' 9 θ 6 θ9 θ 5 θ 2 X 5 i 5 X 9 i 9 X 6 i 6 O 6 l 5 r x 5 x 9 X Figure 9: Sketch for the kinematics of the inner sun gear assembly Let {O 9, X 9, Y 9, Z 9 } be the frame F 9 attached to the planet-carrier, with origin O 9 located at the intersection of the axis of the inner sun gear assembly and its center of mass The orientation of frame F 9 is the same as that of frame F 5 The position vector o 9 of frame F 9 in frame F 5 is given by o 9 = i 5 + j 5 + (h 9 r) k 5, (27) 14

15 where h 9 is the height of the center of mass of the inner sun gear assembly Furthermore, let {O 9, X 9, Y 9, Z 9} be the frame F 9 attached to the inner sun gear assembly, with origin O 9 coinciding with O 9 Let Q 9 be the 3 3 orthogonal matrix that rotates frame F 9 with respect to the frame F 9 around the Z 9 axis through an angle θ 9: cos θ 9 sin θ 9 Q 9 = sin θ 9 cos θ 9 (28) 1 with θ 9 = θ 9 θ 5, ω 9 = ω 9 ω 5 (29) Let N 7,9 denote the ratio of the number N 9 of teeth of gear 9 to the number N 7 of teeth of gear 7 This ratio is also the ratio of the angular velocity of the lower left gear assembly ω 7 to the angular velocity of the inner sun gear assembly ω 9, as observed in frame F 5, which is Combining eqs (23) and (3) yields: N 7,9 = ω 7 (3) ω 9 ω 9 1 = + θ1 (31) N 1,7 N 7,9 The absolute angular velocity of the inner sun gear assembly ω 9 expressed in frame F 9 is given by: ω 9 = ω 5 + ω 9 (32) [ ] r ω 9 = ( l θ 2 θ 1 1 ) + θ1 k 9 (33) 5 N 1,7 N 7,9 The absolute velocity of the inner sun gear assembly v 9 expressed in frame F 9 is equal to the absolute velocity of the planet-carrier v 5 : v 9 = r 2 ( θ 1 + θ 2 ) i 9 (34) We will also need the angular velocity of the inner sun gear assembly ω 9 as observed in frame 15

16 F 6 for the derivation of the angular velocity relations of the upper right gear assembly: ω 9 = ω 9 ω 6 = ω 9 + ω 5 ω 6 = 1 N 1,7 N 7,9 θ1 + ω 5 ω 6 (35) First epicyclic gear train - Upper right gear assembly (11) The variables describing the kinematics of the upper right gear assembly are shown in Fig 1 Y j 11 Y 11 i' 11 j 6 Y 6 θ 1 X' 11 X 6 i 6 j' 11 Y' 11 O 5 θ' 11 θ 11 X 11 i 11 y 11 y 6 O 6 ω 6 v 6 l 11 θ6 O' 11 O 11 θ 6 r θ 2 l x 6 X Figure 1: Sketch for the kinematics of the upper right gear assembly x 11 Let {O 11, X 11, Y 11, Z 11 } be the frame F 11 attached to the chassis, with origin O 11 located at the intersection of the axis of the upper right gear assembly and its center of mass The orientation of frame F 11 is the same as that of frame F 6 while the position vector o 11 of O 11 in frame F 6 is given by: o 11 = l i 6 l 11 j 6 + (h 11 r) k 6, (36) where h 11 is the height of the center of mass of the upper right gear assembly Furthermore, let {O 11, X 11, Y 11, Z 11} be the frame F 11 attached to the upper right gear assembly, with O 11 coinciding with O 11 Let Q 11 be the 3 3 orthogonal matrix that rotates frame F 11 with 16

17 respect to the frame F 11 around the Z 11 axis through an angle θ 11: cos θ 11 sin θ 11 Q 11 = sin θ 11 cos θ 11, (37) 1 with θ 11 = θ 11 θ 6, ω 11 = ω 11 ω 6 (38) Let N 9,11 denote the ratio of the number N 11 of teeth of gear 1 to the number N 9 of teeth of gear 9 This ratio is also the ratio of the angular velocity of the inner sun gear assembly ω 9 to the angular velocity of the upper right gear assembly ω 11, as observed in frame F 6, which is N 9,11 = ω 9, (39) ω 11 The angular velocity of the upper right gear assembly ω 11, as observed in frame F 6, is given by: ω 11 = ω 9 1 = θ1 1 (ω 5 ω 6 ) (4) N 9,11 N 1,7 N 7,9 N 9,11 N 9,11 The angular velocity of the upper right gear assembly ω 11, as observed in frame F 6, corresponds to the velocity sensing of motor 1, ie, ω 11 ω m1, 1 ω m1 = θ1 1 α (41) N 1,7 N 7,9 N 9,11 N 9,11 The angular velocity of the left wheel θ 1 in terms of the velocity sensing of motor 1 is given by: ( θ 1 = N 1,7 N 7,9 N 9,11 ω m1 + 1 ) α (42) N 9,11 The absolute angular velocity of the upper left gear assembly ω 11 as observed in frame F 11 is given by: ω 11 = ω 6 + ω 11 1 = ω 6 θ1 1 (ω 5 ω 6 ), (43) N 1,7 N 7,9 N 9,11 N 9,11 [ ( 1 ω 11 = θ ) r sin α( N 1,7 N 7,9 N 9,11 N 9,11 2l θ 1 + θ 2 ) r ] ( l 5 N θ 2 θ 1 ) k 11 (44) 9,11 17

18 The absolute velocity of the upper left gear assembly v 11 as observed in frame F 11 is given by: v 11 = (v 6 + l 11 ω 6 ) i 11 + l ω 6 j 11, (45) [ r v 11 = 2 cos α( θ 1 + θ 2 ) + rl ] 11 sin α( 2l θ 1 + θ 2 ) i 11 + r 2 sin α( θ 1 + θ 2 ) j 11, (46) Second epicyclic gear train - Front right wheel assembly (2) The variables describing the kinematics of the front right wheel assembly are shown in Fig 11 Y j' 2 Y' 2 j 2 j 5 Y 2 Y 5 θ 1 l 5 ω 5 v 5 X 5 i 5 X 2 i 2 O 5 O' 2 ω 2 O 2 v 2 θ 2 r X Figure 11: Sketch for the kinematics of the front right wheel assembly Let {O 2, X 2, Y 2, Z 2 } be the frame F 2 attached to the planet-carrier, with origin O 2 located at the intersection of the midplane of the right front wheel and the axis of rotation of the same wheel The orientation of frame F 2 is the same as that of frame F 5 while the position 18

19 vector o 2 of the origin of frame F 2 in frame F 5 is given by: o 2 = i 5 l 5 2 j 5 + k 5 (47) Furthermore, let {O 2, X 2, Y 2, Z 2} be the frame F 2 attached to the right front wheel, with origin O 2 coinciding with O 2 Let Q 2 be the 3 3 orthogonal matrix that rotates frame F 2 with respect to frame F 2 around the Y 2 axis through an angle θ 2 : cos θ 2 sin θ 2 Q 2 = 1 (48) sin θ 2 cos θ 2 The absolute velocity v 2 and absolute angular velocity ω 2 of the front right wheel assembly expressed in frame F 2 are given by: v 2 = r θ 2 i 2, (49) ω 2 = θ 2 j 2 + ω 5 k 2 = θ 2 j 2 + r l 5 ( θ 2 θ 1 ) k 2 (5) Second epicyclic gear train - Lower right gear assembly (8) The variables describing the kinematics of the lower right gear assembly are shown in Fig 12 Let {O 8, X 8, Y 8, Z 8 } be the frame F 8 attached to the planet-carrier, with origin O 8 located at the intersection of the axis of the lower right gear assembly and its center of mass The orientation of frame F 8 is the same as that of frame F 5, while the position vector o 8 of the origin O 8 of frame F 8 with respect to frame F 5 is given by: o 8 = i 5 l 8 j 5 + (h 8 r) k 5, (51) where h 8 is the height of the center of mass of the lower right gear assembly Furthermore, let {O 8, X 8, Y 8, Z 8} be the frame F 8 attached to the lower right gear, with origin O 8 coinciding with O 8 Let Q 8 be the 3 3 orthogonal matrix that rotates frame F 8 with respect to the 19

20 Y j j 8 5 Y Y 8 5 j' 8 X' X 5 8 θ 1 v 5 ω5 Y' 8 v θ O 5 8 y 5 5 ω 8 y 8 O 8 θ 2 i' 8 θ' 8 i 5 i 8 X 8 θ 8 l 5 l 8 r x 5 x 8 X Figure 12: Kinematics of the lower left gear assembly frame F 8 around the Z 8 axis through an angle θ 8: cos θ 8 sin θ 8 Q 8 = sin θ 8 cos θ 8, (52) 1 with θ 8 = θ 8 θ 5, ω 8 = ω 5 + ω 8 (53) Let N 2,8 denote the ratio of the number N 8 of teeth of gear 8 to the number N 2 of teeth of gear 2 This ratio is also the ratio of the angular velocity of the right wheel θ 2 to the angular velocity of the lower right gear assembly ω 8, as observed in frame F 8 : N 2,8 = + θ 2 (54) ω 8 Then, the lower right gear angular velocity ω 8 observed in frame F 8 is defined as: ω 8 = + 1 N 2,8 θ2 (55) 2

21 The absolute angular velocity of the lower right gear assembly ω 8 expressed in frame F 8 is defined by: ω 8 = (ω 5 + ω 8) k 8 = ( r ( l θ 2 θ 1 ) + 1 ) θ2 5 N 2,8 k 8 (56) The absolute velocity of the lower right gear assembly v 8, expressed in frame F 8 is defined by: v 8 = (v 5 + l 8 ω 5 ) i 8 = ( r 2 ( θ 1 + θ 2 ) + rl ) 8 ( l θ 2 θ 1 ) 5 i 8, (57) Second epicyclic gear train - Outer sun gear assembly (1) The variables describing the kinematics of the outer sun gear assembly are shown in Fig 13 Y i' 1 y 5 y 1 j 6 Y 6 j' 1 j 5 Y Y 5 1 j 1 Y' 1 θ 1 ω 1 ω 5 O 1 O' 1 O 5 v 5 X' 1 θ' 1 θ'' 1 θ 6 θ1 θ 5 θ 2 X 5 i 5 i 1 X 1 X 6 i 6 O 6 l 5 r x 5 x 1 X Figure 13: Sketch for the kinematics of the outer sun gear assembly Let {O 1, X 1, Y 1, Z 1 } be the frame F 1 attached to the planet-carrier, with origin O 1 located at the intersection of the axis of the outer sun gear assembly and its center of mass The orientation of frame F 1 is the same as that of frame F 5 while the position vector o 1 21

22 of the origin of frame F 1 in frame F 5 is given by: o 8 = i 5 + j 5 + (h 1 r) k 5 (58) where h 1 is the height of the center of mass of the outer sun gear assembly Furthermore, let {O 1, X 1, Y 1, Z 1} be the frame F 1 attached to the outer sun gear assembly, with origin O 1 coinciding with O 1 Let Q 1 be the 3 3 orthogonal matrix that rotates frame F 1 with respect to frame F 1 around the Z 1 axis through an angle θ 1: cos θ 1 sin θ 1 Q 1 = sin θ 1 cos θ 1 (59) 1 with θ 1 = θ 5 + θ 1, ω 1 = ω 5 + ω 1 (6) Let N 8,1 denote the ratio of the number N 1 of teeth of gear 1 to the number N 8 of teeth of gear 8 This ratio is also the ratio of the angular velocity of the lower right gear assembly ω 8 to the angular velocity of the outer sun gear assembly ω 1 as observed in frame F 5 : Combining equations (54) and (61) yields: N 8,1 = ω 8 (61) ω 1 ω 1 1 = θ2 (62) N 2,8 N 8,1 The absolute angular velocity of the outer sun gear assembly ω 1 expressed in frame F 1 is given by: ω 1 = ω 5 + ω 1 = r ( l θ 2 θ 1 1 ) θ2, (63) 5 N 2,8 N 8,1 [ ω 1 = r ( ) ] r 1 θ1 + θ 2 k 1 (64) l 5 l 5 N 2,8 N 8,1 The absolute velocity of the outer sun gear assembly v 1 expressed in frame F 1 is equal to 22

23 the absolute velocity of the planet-carrier v 5 : v 1 = r 2 ( θ 1 + θ 2 ) i 1 (65) We will also need the angular velocity of the outer sun gear assembly ω 1 as observed in frame F 6 for the derivation of the angular velocity equations of the upper left gear assembly: 1 = ω 1 ω 6 = ω ω 5 ω 6 = θ2 + ω 5 ω 6 (66) N 2,8 N 8,1 ω Second epicyclic gear train - Upper left gear assembly (12) The variables describing the kinematics of the upper left gear assembly are shown in Fig 14 Y j' 12 Y' 12 j 12 Y 12 X' 12 i' 12 X 12 i 12 y 12 j 6 Y 6 θ 1 O' 12 O 12 θ' 12 θ 12 θ 6 X 6 i 6 l 12 ω 6 v 6 θ 6 O 5 θ 2 y 6 O 6 r l x 6 X Figure 14: Sketch for the kinematics of the upper left gear assembly x 12 Let {O 12, X 12, Y 12, Z 12 } be the frame F 12 attached to the chassis, with origin O 12 located at the intersection of the axis of the upper left gear assembly and its center of mass The orientation of frame F 12 is the same as that of frame F 5 while the position vector o 12 of the 23

24 origin of frame F 12 in frame F 6 is given by: o 12 = l i 6 + l 12 j 6 + (h 12 r) k 6, (67) where h 12 is the height of the center of mass of the upper left gear assembly Furthermore, let {O 12, X 12, Y 12, Z 12} be the frame F 12 attached to the upper left gear assembly, with origin O 12 coiciding with O 12 Let Q 12 be the 3 3 orthogonal matrix that rotates frame F 12 with respect to the frame F 12 around the Z 12 axis through an angle θ 12: cos θ 12 sin θ 12 Q 12 = sin θ 12 cos θ 12, (68) 1 with θ 12 = θ 12 θ 6, ω 12 = ω 12 ω 6 (69) Let N 1,12 denote the ratio of the number N 12 of teeth of gear 12 to the number N 1 of teeth of gear 1 This ratio is also the ratio of the angular velocity of the outer sun gear assembly ω 1 to the angular velocity of the upper left gear assembly ω 12, as observed in frame F 6 : 1 ω 12 N 1,12 = ω, (7) The angular velocity of the upper left gear assembly ω 12, as observed in frame F 6, is given by: ω 12 = ω 1 N 1,12 = 1 θ2 1 (ω 5 ω 6 ), (71) N 2,8 N 8,1 N 1,12 N 1,12 The angular velocity of the upper left gear assembly ω 12, as observed in frame F 6, corresponds to the velocity sensing of motor 2: ω 12 ω m2, ω m2 = 1 θ2 1 α (72) N 2,8 N 8,1 N 1,12 N 1,12 The angular velocity of the right wheel θ 2 in terms of the velocity sensing of motor 2 is given by: ( θ 2 = N 2,8 N 8,1 N 1,12 ω m2 + 1 ) α (73) N 1,12 24

25 The absolute angular velocity of the upper left gear assembly ω 12 as observed in frame F 12 is given by: ω 12 = ω 6 + ω 12 1 = ω 6 + θ2 1 (ω 5 ω 6 ), (74) N 2,8 N 8,1 N 1,12 N 1,12 [ ( 1 ω 12 = θ ) ] r sin α( N 2,8 N 8,1 N 1,12 N 1,12 2l θ 1 + θ r 2 ) ( l 5 N θ 2 θ 1 ) k 12 (75) 1,12 The absolute velocity of the upper left gear assembly v 12 as observed in frame F 12 is given by: or, upon substitution of the various velocities involved, v 12 = v 12 = (v 6 l 12 ω 6 ) i 12 + l ω 6 j 12, (76) [ r 2 cos α( θ 1 + θ 2 ) rl ] 12 sin α( 2l θ 1 + θ 2 ) i 12 + r 2 sin α( θ 1 + θ 2 ) j 12 (77) Rear left wheel assembly (3) The variables describing the kinematics of the rear left wheel assembly are shown in Fig 15 Let {O 3, X 3, Y 3, Z 3 } be the frame F 3 attached to the chassis, with origin O 3 located at the intersection of the midplane of the rear left wheel and the axis of rotation of the same wheel The orientation of frame F 3 is the same as that of frame F 6 The position vector o 3 of the origin of frame F 3 in frame F 6 is given by: o 3 = i 6 + l 6 2 j 6 + k 6 (78) Furthermore, let {O 3, X 3, Y 3, Z 3} be the frame F 3 attached to the rear left wheel assembly, with O 3 coinciding with O 3 Let Q 3 be the 3 3 orthogonal matrix that rotates frame F 3 with respect to frame F 3 around the Y 3 axis through an angle θ 3 : cos θ 3 sin θ 3 Q 3 = 1 (79) sin θ 3 cos θ 3 25

26 Y j' 3 j 3 Y 3 j 6 Y 6 Y' 3 r O' 3 θ 3 O 3 X 3 i 3 ω 6 v 6 X 6 i 6 y 6 l 6 x 6 X Figure 15: Sketch for the kinematics of the rear left wheel assembly The absolute velocity of the rear left wheel assembly v 3, as observed in frame F 3, is given by: v 3 = ( v 6 l ) 6 2 ω 6 i 3 = [ r 2 cos α( θ 1 + θ 2 ) rl ] 6 sin α( 4l θ 1 + θ 2 ) i 3 (8) The absolute angular velocity of the rear left wheel assembly ω 3, observed in frame F 3, is given by: ω 3 = v 3 r j 3 + ω 6 k 3, (81) [ 1 ω 3 = 2 cos α( θ 1 + θ 2 ) l ] 6 sin α( 4l θ 1 + θ 2 ) j 3 + r sin α( 2l θ 1 + θ 2 ) k 3 (82) Rear right wheel assembly (4) The variables describing the kinematics of the rear right wheel assembly are shown in Fig 16 Let {O 4, X 4, Y 4, Z 4 } be the frame F 4 attached to the chassis, with origin O 4 located at the intersection of the midplane of the rear right wheel and the axis of rotation of the same 26

27 Y j' 4 j 4 Y 4 j 6 Y 6 Y' 4 O 3 ω 6 v 6 X 6 i 6 y 6 l 6 x 6 O' 4 O4 r θ 4 X 4 i 4 X Figure 16: Sketch for the kinematics of the rear right wheel assembly wheel The orientation of frame F 4 is the same as that of frame F 6 The position vector o 4 of the origin of frame F 4 in frame F 6 is given by: o 4 = i 6 l 6 2 j 6 + k 6 (83) Furthermore, let {O 4, X 4, Y 4, Z 4} be the frame F 4 attached to the rear right wheel assembly, with origin O 4 coinciding with O 4 Let Q 4 be the 3 3 orthogonal matrix that rotates frame F 4 with respect to the frame F 4 around the Y 4 axis through an angle θ 4 : cos θ 4 sin θ 4 Q 4 = 1 (84) sin θ 4 cos θ 4 The absolute velocity of the rear right wheel assembly v 4 observed in frame F 4 is given by: v 4 = ( v 6 + l ) 6 2 ω 6 i 4 = [ r 2 cos α( θ 1 + θ 2 ) + l ] 6r sin α( 4l θ 1 + θ 2 ) 27 i 4 (85)

28 The absolute angular velocity of the rear right wheel assembly ω 4 observed in frame F 4 is given, in turn, by: ω 4 = v 4 r j 4 + ω 6 k 4, (86) [ 1 ω 4 = 2 cos α( θ 1 + θ 2 ) + l ] 6 sin α( 4l θ 1 + θ 2 ) j 4 + r sin α( 2l θ 1 + θ 2 ) k 4 (87) 22 Dynamics The dynamics model of the mechanical system under study is of the general form [5]: I θ a + C θ a = τ, (88) with τ [ τ 1 τ 2 ] T, (89) while I is the 2 2 generalized inertia matrix, C is the 2 2 the generalized matrix of Coriolis and centrifugal forces and τ is the vector of torques delivered to the two front wheels In deriving the equations of motion, we distinguish between the output torque of the motors which we call the motor torques and the torque that is delivered to the wheels which we call the wheel torques We define the vector of motor torques as: τ m [ τ 1 τ 2] T, (9) where τ 1 and τ 2 are the torques delivered by the two motors The torques are related through equation: τ 1 = N 1,m1 τ 1, N 1,m1 N 1,7 N 7,9 N 9,11, (91) τ 2 = N 2,m2 τ 2, N 2,m2 N 2,8 N 8,1 N 1,12, (92) The wheel torques are used to derive the equations of motion of the robot 28

29 The expression for the generalized inertia matrix I is derived from the kinetic energy of the ith rigid body, namely: T i = 1 2 tt i M i t i, (93) where M i is the constant 6 6 inertia dyad of the ith rigid body defined in terms of the 3 3 inertia matrix J i and the mass m i of the ith rigid body: [ ] J xi Ji 3 3 M i, J i 3x3 m i 1 J yi (94) 3 3 J zi where J i is expressed in the same reference frame as the twist t i of the ith rigid body Upon introduction of relations (4), the above expression becomes T i = 1 2 θ T a (T T i M i T i ) θ a, (95) a quadratic form in the vector of wheel rates, whose matrix is the inertia matrix of the ith body, namely, I i = T T i M i T i (96) The inertia matrix of the overall mechanical system is simply the sum of all 12 body-matrices of inertia: 12 I = T T i M i T i, (97) i=1 The generalized matrix of the Coriolis and centrifugal forces C i, associated with the ith body, is defined as: C i = T T i M i Ṫ i + T T i W i M i T i, (98) while the system counterpart matrix is simply the sum of all 12 body-matrices C i, ie, 12 C = (T T i M i Ṫ i + T T i W i M i T i ), (99) i=1 with Ṫi denoting the time-derivative of the 6 2 twist-shaping matrix of the ith rigid body 29

30 and W i is the 6 6 angular velocity dyad of the ith rigid body, which is given by: [ ] ω zi ω yi Ωi 3 3 W i, Ω i 3 3 ω zi ω xi (1) 3 3 ω yi ω xi Since ω i is a function of α and θ a, W i is also a function of α and θ a : W W(α, θ a ) (11) For some of the 12 assemblies, namely, the rear left wheel, the rear right wheel, the chassis, the upper left gear and the upper right gear assemblies, the twist-shaping matrices T i are functions of α Their time differentiation Ṫi then becomes a function of α and α, ie, Ṫ i = Ṫi(α, α) (12) However, α is a function of α and θ a, namely, α = ω 5 ω 6 α = r l 5 ( θ 2 θ 1 ) r 2l ( θ 1 + θ 2 ) sin α (13) Using eq(13) the time-derivative of the twist-shaping matrix of the ith rigid body Ṫi can now be expressed as a function of α and θ a : Ṫ i Ṫi(α, θ a ) (14) For the other assemblies, namely, the front left wheel, the front right wheel, the chassis, the lower left gear, the lower right gear, the inner sun gear and the outer sun gear, the twist-shaping matrices are constant so their time differentiation equals zero The mathematical model of the robot thus reduces to: τ = I(α) θ a + C(α, θ a ) θ a, (15) 3

31 where I(α) and C(α, θ a ) are defined as: 12 I(α) = T i (α) T M i T i (α) (16) i=1 12 C(α, θ a ) = i=1 [ T i (α) T M i Ṫ i (α, θ a ) + T i (α) T W i (α, θ ] a )M i T i (α) (17) Note here that the term C(α, θ a ) θ a clearly shows the quadratic nature of the generalized Coriolis and centrifugal terms with respect to the joint rates As the foregoing generalized matrix arises from the presence of α, only the rear left wheel, the rear right wheel, the chassis, the upper left gear and the upper right gear assemblies contribute to this term Furthermore, since the chassis, the upper left gear and the upper right gear assemblies undergo planar motion, only the two rear wheel assemblies contribute to the term T T i W i M i T i [5] The inertial and mass parameters of each of the 12 rigid-body assemblies are derived using SolidWorks, with the mass density of the Acrylonitrile Butadiene Styrene (ABS) LEGO parts and the stainless steel modified axles, and by weighing the other parts of the robot (battery, motors, mechatronics) The mass of the motors is included in the mass of the chassis assembly while the inertia of the 2 motor rotors is added to the corresponding upper gear assemblies The total mass of the robot was also compared to the total simulated mass of the robot under SolidWorks to validate the model Finally, the equations of motion of the robot were derived in terms of the 12 rigid body assemblies using Maple The evaluated generalized inertia matrix I(α) is given by: [ ] I1,1 I 1,2 I(α) =, (18) I 1,2 I 2,2 I 1,1 = ( cos 2α sin α) 1 3, I 1,2 = ( cos 2α) 1 3, I 2,2 = ( cos 2α sin α) 1 3, all with units of Kg m 2 We note here the symmetry of the generalized inertia matrix The 31

32 entries of the generalized inertia matrix are shown in Appendix A Because of the difference in mass and inertia of the inner and outer sun gear assemblies, the inertia matrix terms I 1,1 and I 2,2 are different The effort needed to accelerate the robot is higher at an angle α of and π corresponding to forward and reverse motion of the robot than the effort needed to accelerate the robot at an angle α of π/2 and 3π/4 corresponding to a pure rotation of the chassis The evaluated matrix of generalized Coriolis and centrifugal fores C(α) is given by: [ ] C(α, θ C1,1 C 1,1 a ) =, (19) C 2,2 C 2,2 C 1,1 = ( sin 2α cos α 9941 cos 3α) 1 5 θ1 + (78154 cos α 9941 cos 3α sin 2α) 1 5 θ2, C 2,2 = ( sin 2α cos α 9941 cos 3α) 1 5 θ1 + (12648 cos α 9941 cos 3α sin 2α) 1 5 θ2, all with units of Kg m 2 /s Using the equations of motion derived in this section, we can now proceed to the definition of a control algorithm 32

33 3 Control Our aim here is to design a controller using pole-placement and state-feedback while taking into account that the robot is configuration-dependent in terms of α 31 State-Space Form The first step in defining a proper control is to express the equations of motion of the system in state-space form [6], [7] We define the state vector of the system as x θ [ ] T a = θ1 θ2 (11) while the desired state vector is defined as x d [ θ 1 θ 2] T, (111) where θ 1 and θ 2 are the desired actuated joint rates of the robot The controlled variables are defined as the velocity and angular velocity of the planetcarrier assembly, ω 5 and v 5 We thus define the desired output vector y d, also known as the reference input vector of our system, as y d the actual output vector of the system being y We can express the reference input vector y d [ v 5 ω 5] T, (112) [ v 5 ω 5 ] T (113) and the actual output vector y as linear transformations of the desired state vector and the state vector, respectively, with: y d = Hx d, y = Hx, H = r 2 r 2 r (114) r l 5 l 5 where H is the state output matrix 33

34 The dynamics model of the mechanical system is given by I(α) θ a + C(α, θ a ) θ a = τ (115) Solving for θ a from the above model yields: θ a = I 1 (α)c(α, θ a ) θ a + I 1 (α)τ (116) The dynamics model can then be expressed in state-space form as: ẋ = l(x, u), y = Hx, (117) with u [ τ 1 τ 2 ] T, l(x, u) = I 1 (α)c(α, x)x + I 1 (α)u, (118) where u is the input vector of the system 32 Model Linearization In order to linearize the state space model, we resort to Lyapunov s linearization method (LLM) [6] To do this, we linearize the model around an equilibrium state by setting l(x, u) = 2, with u = 2, x = 2, (119) where k is the k-dimensional zero vector This state represents the robot at rest Linearizing the state-space model at the equilibrium state yields ẋ = Fx + Gu, y = Hx, (12) with F l x = O 2 2, G l x=x u = I 1 (α), (121) x=x where F is the state matrix and G is the input matrix of the system 34

35 Note here that linearizing the model at a rest state is equivalent to neglecting the generalized matrix of the Coriolis and centrifugal forces that arises from the presence of α 33 Open-Loop System In order to design a controller using pole placement, the open-loop system is first analyzed in the s-domain of the Laplace Transform The open-loop poles of the linearized equations of motion are the eigenvalues of the system matrix F: p = eig(f) = eig(o 2 2 ), (122) whence the two open-loop poles of the system are {, } and, as expected, the open-loop system is marginally stable (poles are on the imaginary axis), because dissipative forces are not included in the model 34 State Feedback We now derive the state feedback control law based on pole-placement We want the closedloop poles of the system to be located in the left half of the s-plane To this end, we use a control law of the form: u = Kx (123) where K is the 2 2 state feedback matrix, which affects the location of the closed-loop poles [6] Substituting the feedback law given by Eq(123) into the system described by Eq(12) yields ẋ = Fx GKx (124) The block diagram for the control law of the system is shown in Fig 17 u x = Fx + Gu x H y u = - Kx Figure 17: Block diagram for the control law of the system 35

36 The closed-loop poles of the system correspond to the eigenvalues of system (124): p = eig(f GK), (125) Moreover, we introduce a gain matrix, which adapts to the configuration parameter α: K ρi(α) (126) which leads to the characteristic equation (s + ρ)(s + ρ) = (127) Therefore, the two closed-loop poles of the system are located on the real axis at { ρ, ρ} By selecting ρ >, the closed-loop poles are placed on the left-hand side of the complex plane, thereby keeping the system stable Furthermore, the dominant poles of the closedloop system are designed to be real so as to have an aperiodic time response We end up with a control law that adapts to α By adjusting ρ we can tune the time-response to the desired output vector y d of the system We need to find a proper balance between the performance of the system and the effort of the actuators A higher ρ corresponds to poles farther to the left in the s-plane and to a faster time-response combined with a higher torque required from the motors 35 Reference Input The shape of the state feedback control law allowing reference tracking is u = Kx + Ny d, N = (N u + KN x ) (128) where N is the gain matrix that guarantees a reference input tracking [6] The bloc diagram for the control law of the system with reference tracking is shown in Fig 18 Matrices N x and N u are given below in block-form [7]: D [ Nx N u ] ] = [ , D = 36 [ ] F G, (129) H O 2 2

37 y + d N u x = Fx + Gu x H y - K Figure 18: Bloc diagram for the control law of the system with reference tracking where D is required to be invertible While F = O 2 2, but since G and H are invertible, D can be shown to be invertible as well Indeed, by means of a swapping of the two left-hand blocks of D with their right-hand counterparts, a block diagonal matrix is obtained, whose determinant is the product of the determinants of its two diagonal blocks Hence, the gain matrices become N u = 2 2, N x = H 1, N = KH 1 (13) The state feedback control then follows: u = Kx + KH 1 y d, (131) or, equivalently, u = K(H 1 y d x), u = K(x d x) (132) The simplified bloc diagram for the control law of the system with reference tracking is shown in Fig 19 y H -1 x d + d K u x = Gu x H y - x Figure 19: Simplified bloc diagram for the control law of the system with reference tracking 37

38 36 Simulation Runs In order to validate the controller and analyze the dynamic performance of the system, simulation runs were conducted using Simulink The model is shown in Fig 2 α α Circle y_d Reference Input 5*pi y d x d x d = H -1 y d y α d d x d5 x d5 init y d5 θ y d5 d5 init x d6 θ y d6 d5 init θ d6 1 ρ u = K (x d - x) K = ρi(α) Feedback Control x u l(x,u) = - I -1 (α) C(α,x) x + I -1 (α) u Direct Dynamics x ẋ 1 s x y = Hx 5*pi y α x 5 x 5 init y 5 θ y 5 5 init x 6 θ y 6 5 init θ 6 α ω 5 init 6 Actual Kinematics α d5 init ω d6 Desired Kinematics Figure 2: Simulink model The model uses the configuration-dependent feedback control and the state-space equations of the system The direct dynamics block is a Variant Subsystem Block and can be switched between full state-space dynamics and linearized state-space dynamics As mentioned earlier, the linearized system is equivalent to neglecting the generalized matrix of Coriolis and centrifugal forces that arises from the presence of α The actual and desired kinematics of the planet-carrier and the chassis are included in the model The two blocks use the reference input vector y d and the output vector y to derive the kinematic variables The two blocks use the kinematic equations described in this report The values are integrated over time from the initial position of the robot The inside of the kinematics block is shown in Fig 21 The model needs the initial coordinates of the planet-carrier, x 5 init and y 5 init, its initial angle, θ 5 init, and the initial angle between the planet-carrier and the chassis, α init The initial position of the robot is shown in Fig 22 Note that the 3D position of all 12 moving assemblies could be determined in this manner using the kinematic equations described in 38

39 this report y_5 To Workspace1 1 y 4 theta_5_init 5 alpha_init v_5 w_5 1 s x o Integrator 5*pi initial theta_6_init sin(u) sin1 Product1 3 y_5_init v_5 cos(u) 2 cos1 Product x_5_init sin(u) sin2 1/172 omega_6 1/l_ Product2 1 s x o y_integ 1 s x o x_integ 1 s x o theta_6_integ theta_6 alpha cos(u) cos2 172 alpha To Workspace2 3 y_5 x_5 To Workspace 2 x_5 4 theta_5 1 alpha 8 omega_6 7 theta_6 5 x_6 l_ Product3 sin(u) sin y_6 l_1 Product4 Figure 21: Simulink kinematics block The reference input block outputs the desired planet-carrier velocity v d5 and angular velocity ω d5 combined in the reference input vector y d5 The state output matrix H is used to transform the desired planet-carrier velocity v d5 and angular velocity ω d5 to the desired left and right wheel angular velocities, θ d1 and θ d2 Five different reference inputs were implemented: step-input trajectory, straight trajectory, rotation of the planet-carrier trajectory, circle trajectory and parking trajectory These can be selected using the reference input Variant Subsystem Block The trajectories can be modified inside the blocks by adjusting different parameters such as the amplitude, the maximum velocity, the maximum acceleration, the task time, the trajectory angle, etc The straight trajectory, rotation of the planet-carrier trajectory and circle trajectory were implemented using degree polynomials whereas the parking trajectory was implemented using a cycloidal path 39

40 Y θ 1 ω 5 v 5 θ 5 init θ2 O 5 X O 6 Figure 22: Initial position of the robot in the Simulink model; x 5 init = m; y 5 init = m; θ 5 init = π/2 rad; α init = rad The gain ρ can be adjusted inside the model As mentioned earlier, adjusting ρ tunes the time-response to the desired output vector y d of the system A higher ρ corresponds to higher wheel torques, τ 1 and τ 2, and a decreased wheel velocity and path error For the step-input, we can use the maximum allowed wheel torque and the initial velocity error to find the maximum allowed gain parameter ρ For the other reference inputs, the maximum torque will depend on the maximum trajectory acceleration The gain parameter ρ can be higher for these reference inputs and will ultimately depend on the discrete time-domain implementation The desired wheel angular velocities, θd1 and θ d2, are saturated in the Simulink model The prototype was designed to be driven at a maximum of 1 m/s With a wheel radius of 4 m, the maximum angular velocity of the wheels is thus 25 rad/s The desired wheel torques, τ 1 and τ 2, are also saturated in the Simulink model The maximum continuous motor rotor torque of the two motors is 66 Nm With a planetary gearhead gear ratio is 19:1 (3249:169), the maximum motor shaft torque is 1165 Nm Finally, with a DWT gear ratio of 1:1, the maximum wheel torque is 1165 Nm 4

41 The Simulink model outputs the variables to the MATLAB workspace A post-load MATLAB function runs automatically after each run It reads the variables from the MAT- LAB workspace and outputs a figure with the results of the simulation Results for the five different reference inputs are shown in Appendix B Limits of the different parameters are shown on the figures The results show that the configurationdependent feedback controller is appropriate for the system and allows for an aperiodic time response In particular, for the step-input, the torque limit is respected by setting ρ = 278 s 1 Also, as expected, the torque required for the rotation of the planet-carrier is much lower than the one required for the straight path trajectory since most of the weight is in the chassis Part of the deviations from the circle trajectory and the parking trajectory arise from the presence of the matrix of generalized Coriolis and centrifugal forces The feedback controller underestimates the torque required to compensate the presence of α 4 Mechatronics We now describe the mechatronics system of the robot prototype The motors used for the prototype are two 21-V RE-Max Maxon motors (model number 22131) These are coupled to GP22A Maxon planetary gearheads (model number 11338) and Maxon magnetic quadrature encoders (model number 21937) with 512 counts per turn and 2 output channels We use the Arduino platform to implement the wireless control of the DWT prototype Arduino is an open-source electronics prototyping platform The Arduino Uno board, shown in Fig 23a, is used as the main component of our mechatronics system The Arduino Uno is a 8-bit microcontroller that is programmed with the Arduino software using the Arduino language, similar to the C language The board can connect to a computer via a USB port The robot control program is uploaded via this USB port The port also allows a serial output while the program is running This allows for data-collection via USB for simulation 41

42 (a) (b) Figure 23: (a) Arduino Uno (b) USB host shield The Arduino platform allows the stacking of electronic shields on top of the Arduino Uno to add functionality to the mechatronics system The Arduino Uno and the shields have input and output pins to communicate, send and read signals There are 14 input/output digital pins numbered from to 13 and six analog input/output pins numbered from A to A5 available We use three shields on top of the Arduino Uno for the wireless control of the two motors The Arduino Uno requires a 9-V supply voltage available either from a standard 9-V battery or via a 9-V wall adapter (a) (b) Figure 24: (a) Logitech Freedom 24 cordless joystick (b) Receiver The first shield is a USB host shield from Sparkfun It allows the Arduino Uno to read data from a joystick wirelessly The USB host shield is shown in Fig 23b We also use a 42

43 Freedom 24 cordless joystick from Logitech, shown in Fig 24a It communicates wirelessly with a USB receiver, shown in Fig 24b The receiver is connected via a USB port to the USB host shield The shield allows the Arduino Uno to read the position of the joystick and the state of its 1 buttons The joystick and buttons are used to control the robot The joystick requires 3 AA batteries to operate (a) (b) Figure 25: (a) Rugged motor driver shield (b) LS7184 encoder-to-counter interface chip The second shield is a motor driver shield from Rugged Circuits, shown in Fig 25a It incorporates two Texas Instruments DRV881 interface chips These are motor drivers that allow pulse-width modulation (PWM) control of the two motors It is capable of peak output currents of up to 28 A and operating voltages up to 36 V, which is enough for the two 21-V Maxon motors It also allows current reading for feedback on the two DC motors The connections of the shield were rewired to desired input and output pins of the Arduino Uno, which has also been modified to increase the sensitivity to current by removing two of the three resistors on the current output reading electronics The Arduino Uno sends PWM signals, from V to 5 V, to the motor drive shield through two pins along with two high or low digital signals for the motor voltage sign These are then converted to voltage outputs by the motor drive for the motors The standard PWM output frequency of the Arduino board, which has to be modified in the program in order to control the motors, was set to 3125 khz A 222-V battery by ThunderPower (model number TP225-6SP3) is connected to the motor drive in order to power the two motors 43