Modeling Dynamics and Exploring Control of a Single-Wheeled

Modeling Dynamics and Exploring Control of a Single-Wheeled Dynamically Stable Mobile Robot with Arms Eric M. Schearer

CMU-RI-TR-06-37

August 31, 2006

Robotics Institute Carnegie Mellon University Pittsburgh, Pennsylvania 15213 c Carnegie Mellon University

Submitted in partial fulfillment of the requirements for the degree of Master of Science

Abstract This paper focuses on simulations of a dynamically stable mobile robot (Ballbot) with arms. The simulations are of Ballbot lifting its arms in various directions. A PD arm controller works independently of an LQR-designed balancing/station keeping controller. The PD controller drives the arms to follow desired trajectories. When the arms are raised, Ballbot assumes a leaning equilibrium (the physical equilibrium) as opposed to the standing equilibrium (body stands totally upright - a predefined desired equilibrium) that the LQR drives toward. The conflict between these two equilibria causes the robot to lose its balance when lifting heavy (10 kg) loads. A unified arm and station keeping/balancing controller is also described. The unified controller outperforms the independent controllers in some cases. Balancing only using arms and driving body movement with arms are briefly explored.

I

Acknowledgments Thanks to my advisor, Dr. Ralph Hollis for his vision in creating Ballbot and guiding my work. Thanks to Dr. George Kantor for constant technical advice and brainstorming. Thanks to Anish Mampetta for his companionship and ideas through all of our experimenting with Ballbot. Anish was also the first to derive the Ballbot motion equations using the current coordinates. Thanks to Dr. Matt Mason and Jonathan Hurst for reviewing my work as part of my committee. Thanks to my family, friends, and classmates for every day support. This research is supported in part by NSF grant IIS-030867. The United States Air Force funds my tuition. Thanks to Col(ret.) Chuck Wolfe, my supervisor at the Air Force Research Lab Munitions Directorate and to the Engineering Mechanics Department at the U.S. Air Force Academy for making the Air Force support possible.

II

Contents 1 Introduction

1

2 Modeling and Simulation 2.1 Simulation in SimMechanics . . . . . 2.1.1 SimMechanics Block Diagram 2.1.2 Friction Model . . . . . . . . 2.1.3 Actuation Model . . . . . . . 2.2 2D Ballbot with Arms Model . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

3 3 3 5 9 10

3 Independent Balancing and Arm Controllers

13

4 Unified Balancing and Arm Controller

15

5 Performance 5.1 Arm Controller Performance . . . . . . . 5.2 Balancing When Arms Move . . . . . . . 5.3 Compare Independent to Unified Control 5.4 New Ideas . . . . . . . . . . . . . . . . .

16 17 19 27 39

. . . . . . . . . . . . Approach . . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

6 Conclusions and Future Work

41

7 References

43

A SimMechanics Block Diagrams A.1 Independent Controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A.2 Unified Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

44 44 49

III

1

Introduction

Ballbot is a mobile robot with human-like height, width, and weight. It actively balances and moves on a single wheel using closed loop feedback, making it dynamically stable. Dynamic stability affords it advantages in maneuverability over statically stable robots and makes it a good candidate for operating in human environments. Balancing on a ball allows Ballbot to be omni-directional, allowing it to move in any direction without turning. The current version of Ballbot has no arms and is described in detail in [4] and [5]. Ballbot’s structure is three aluminum channels held together by circular decks that rest on top of a single ball as seen in Figure 1. Figure 1a shows Ballbot standing with its three legs (used when not dynamically balancing) deployed. On the circular decks are a battery, a computer, a battery charger, and an inertial measurement unit (IMU) for measuring the tilt angle of the body as seen in Figure 1b. Figure 1c. shows Ballbot balancing.

Figure 1: The drive mechanism in Figure 2 includes four motors that drive the rollers which drive the ball. Encoders on the motors measure the position of the rollers and thus the position of the ball. A feedback controller allows Ballbot to move and balance. Future versions of Ballbot will include arms with two degrees of freedom and will look something like Figure 3. Arms will give Ballbot the capability to manipulate objects. Thibodeau et. al. [9] have shown that dynamically stable mobile robots with arms can apply larger contact forces than similar statically stable mobile manipulators. Arms can also increase the stability and mobility of Ballbot. The objectives of this work are:

1

servomotor

motor encoders

belt tensioner ball transfers (3)

drive belt drive roller

ball

Figure 2:

Figure 3: • Create a 3D mechanical simulation of Ballbot with arms to study the dynamics of the armed configuration. Section 2 discusses this model. • Design a simple arm controller to operate simultaneously with and independently of a body station keeping/balancing controller. This is seen in Section 3. • Design a unified station keeping/balancing and arm controller that uses feedback from the arms, ball, and body to both balance and move the arms. Section 4 describes this controller. • Section 5 evaluates and compares the performances of these two controllers. • Section 6 suggests future work.

2

2

Modeling and Simulation

This section describes a 3D Ballbot simulation in SimMechanics and a planar Ballbot model used for deriving controllers.

2.1

Simulation in SimMechanics

A 3D simulation of Ballbot with arms was created using SimMechanics. SimMechanics r . SimMechanics uses the Newtonis a mechanical modeling package used with Simulink Euler equations to describe the motion of rigid bodies and allows users to create models in r block environment. Rigid bodies are modeled by a center of mass position the Simulink and body orientation relative to some coordinate system (a fixed coordinate system or a coordinate system of a connected body), a mass, and an inertia tensor. The user can create degrees of freedom by connecting bodies with joints and take away degrees of freedom with constraints. Virtual sensors connected to bodies and joints allow the user to measure outputs and feed them back to controllers which send signals to actuators that can apply forces, torques, or prescribed motions to joints and bodies. 2.1.1

SimMechanics Block Diagram

Figure 4 shows a block diagram modeling the Ballbot with arms in SimMechanics. The model consists of nine rigid bodies (the ground, the ball, the body, two arms, and four drive rollers), the joints connecting them, the constraints limiting their motion, and two controllers that sense movement and apply torques to the bodies. Orange blocks represent bodies, green blocks represent joints, red blocks represent constraints, blue blocks represent controllers, and the cyan block represents the fixed ground. The “Ball” is connected to “Ground1” by a six degree of freedom joint. The 6-DoF joint is three perpendicular prismatic axes and a quaternion representing rotation. The “Rolling Constraint” block constrains the relative motion of the ball with respect to the ground. The rolling constraint equation is:  g   g  vbx rb ωby g  g   vby =  rb ωbx g vbz 0 where vb and ωb are the linear and angular ball velocities, rb is the ball radius, the z axis is vertical, and the x and y axes are parallel to the ground. These constraints prescribe that the ball rolls on the ground without slipping and does not move in the vertical direction. Superscripts indicate the coordinate system where g stands for ground. Subscripts refer to bodies and vector components. The “Body” which represents the Ballbot’s tower of motors, sensors, computers, etc. is connected to the “Ball” by a spherical joint represented by a quaternion. Revolute joints, defined by an axis and rotation angle, connect the four rollers (“y1 roller”, “x1 roller”, “y2 roller”, and “x2 roller”) to the “Body”. The red block, “Roller Constraints”, constrains the relative motion of the rollers and ball. These constraint equations are:

3

Act Y1 Sense Y

Env

B

F Act X1

Machine Environment

Ground1

Six−DoF

CS2

B

F

Sense X

CS6 Act Y2

CS7

Spherical

CS4

Conn1

CS2

CS10

Sense Body Act X2

4

Figure 4: SimMechanics Block Diagram

CS5

Conn2

CS9

Control and Actuation CG

CS1

Conn3

CS1 CS4

CS8

B

CS2 F

Rolling Constraint

Ball

Conn1

CS1

Conn2

y1 roller Revolute CG CS3

B

CS2 F

Conn3

CS1 Conn4

CS1

F

B

CS2

CS1

F

B

x1 roller

CS8

Revolute1 CG CS2

hand1

arm1

Weld

CS6

B

F

Universal

Conn5

CS1 y2 roller

Revolute2

Conn6

CG CS2 CS5

B

F

Conn7

CS1 x2 roller

Revolute3 CS1

F

B

CS2

CS1

F

B

CS9 Conn9

hand2

Weld1

arm2

Conn8

CS7 Universal1

Roller Constraints Body

Act Arm1

Sense Arm1

Act Arm2

Sense Arm2

Arm Controller

g x1r x2r rx1r ωx1rz = rb ωby = rx2r ωx2rz y1r g y2r ry1r ωy1rz = rb ωbx = ry2r ωy2rz

where x1r, x2r, y1r, and y2r, refer to the rollers and b refers to the ball. The z axis of each roller is its rotation axis. These constraints mean that the rollers roll without slipping on the ball. The rollers can slip on the ball in the non-rolling directions. For example, in Figure 2, if the ball is rolling on the roller labeled “drive roller”, it must be slipping on the roller perpendicular to the “drive roller” (the other visible roller). The second roller still will roll on the ball without slipping when it is rotating. Note that the ball angular velocity is expressed in the ground frame when it should be expressed in the corresponding roller frame. A major flaw of SimMechanics is that it only allows the user to express constraint equations in either the body frame or the ground frame and not in the coordinate frame of another body. Here we want to express the ball’s angular velocity in the roller frame. With these limitations, if Ballbot yaws, the constraints are no longer correct (making the simulation somewhat invalid) because the ground frame no longer lines up with the roller frames. The arms, “arm1”, and “arm2” are connected to the “Body” by universal joints, which allow two perpendicular degrees of rotational freedom. Based on the Ballbot’s height (about 1.5 m), Ballbot’s arms are 0.58 m long - proportional to a human. The arms are cylinders with the density of aluminum. The hands “hand1” and “hand2” are spheres welded to the ends of the arms. The blue “Control and Actuation” block senses the orientation of the “Body” and positions of the rollers (and the arms in one controller) and applies torques to the rollers. The “Arm Controller” block senses the position of the arms (and the body orientation and roller positions in one controller) and applies torques to the arms. The appendix discusses these two blocks in more detail. Section 3 describes the controllers in detail. 2.1.2

Friction Model

The friction model is found in the “Control and Actuation/Actuation and Friction” block of Figure 4. No friction is modeled in the arms, and no friction is explicitly modeled between the ball and ground and between the ball and body. The ball/ground, ball/body, and ball/roller friction torques are lumped together and implemented by subtracting friction torques from the torque applied to the rollers as follows: τ = τr − τs − τv − τc

(1)

where τ , τr , τs , τv , and τc , are the total torque applied to the roller, the torque that the controller prescribes, the torque on the roller due to static friction, the torque on the roller due to viscous friction, and the torque on the roller due to coulomb friction, respectively. The friction torques are further defined below. τr if τr ≤ τsm (2) τs = 0 if τr > τsm where τsm is the maximum static friction torque. 5

τv = γv ωrz

(3)

where ωrz is the angular velocity of the roller about its rotation axis. τc = γc sign(ωrz )

(4)

To determine values for τsm , γv , and γc , a series of Ballbot tests were conducted. The tests were conducted in the first floor hallway of Smith Hall on the black ribbed carpet. Ballbot rolled on a hollow aluminum ball with urethane coating. No controller was used. For each test, predetermined torque commands were given to the motors. Without a controller to keep Ballbot standing, people held Ballbot vertical as it moved. The first test series used a slow ramping (increment torque input by 0.0785 Nm or 5 DAC counts per second) of the input torque. For much of each of these runs the torque was not enough to cause any ball movement. Eventually enough torque was applied to move the ball. This first series determined the static friction torque. The next set of tests used faster increases to the input torque (1.57 Nm or 100 counts per second) to provide data to estimate the viscous and coulomb friction torques. We ran tests in positive and negative x and y directions and in the diagonal directions as well. The maximum static friction torque for each test run is the value of τr when the ball velocity becomes non-zero. By looking at the data (ball velocity vs. time plots), we can determine the maximum static friction. For example, in Figure 5a the ball appears to move shortly after 3 seconds. We know that the rollers applied 4.71 Nm to the ball at that time. This torque is the maximum static friction for the particular test run. The ball velocity threshold used was 0.1 rad/s. τv and τc were determined by assuming a quasi-steady state for the region of the data where the velocity is non-zero, namely: τr − τv − τc = 0

(5)

substituting equations (3 - 4) into equation (5) and rearranging terms: γv ωrz + γc = τr

(6)

The process for finding γv and γc is outlined below: • Fit a line to the ball velocity vs. time plot in the non-zero velocity region. • Compute the value of the ball velocity at the end of the test run using the best fit line. • Plug this final ball velocity (converted to roller velocity) into equation (6) for each test. • Now we have a system of n (number of test runs for each direction) equations in two unknowns, γv and γc . Solve these equations using singular value decomposition (SVD). • An alternate solution method was using two ball velocity values from the best fit line and solving a system of 2 equations and 2 unknowns for each test run. Then average the γv and γc values for each test run over the set of similar (same ball velocity direction) test runs. 6

Table 1 shows the values of each of the friction terms for a number of different directions (e.g. +x-y is the direction 45 degrees from the positive x and 45 degrees from the negative y) and for all directions. The fact that the values vary across the different directions suggests that friction is not isotropic. Direction τsm (AVG) +x 4.91 -x 5.06 +y 4.3 -y 5.38 +x+y 4.35 +x-y 7.04 -x+y 4.35 -x-y 5.55 All 5.01

γv (AVG) 2.05 2.18 1.56 2.01 0.96 1.84 0.61 2.74 2.08

γc (AVG) 5.45 5.60 5.30 6.41 5.82 7.06 6.08 6.02 5.61

γv (SVD) 2.11 0.91 0.61 1.12 0.96 1.75 0.58 1.48 0.57

γc (SVD) 5.42 6.38 6.72 8.58 5.82 7.33 6.46 6.25 8.88

Table 1: Model Coefficients Note that the data in Table 1 are for ball torques which were used for a previous simulator. The current simulator uses roller torques so the appropriate conversion must be made. Also note that the test data was taken when Ballbot had only two driven rollers (now all four are driven). To account for more driven rollers, the current simulation uses the Table 1 values divided by 2. This may be a poor assumption, and the tests should be run again to obtain more accurate friction terms. To check the goodness of the friction terms in Table 1, a 1D simulation of the ball motion ignoring the effects of the body was created. The simulation solves the model equation (7) using the new friction values in Table 1. The moment of inertia of the ball is Ib = 0.0463 kg m2 , and the angular acceleration of the ball is θ¨b . Plots of the simulation output with the corresponding plots of hallway test runs are displayed in Figure 5. In the legend of Figure 5, “drive1” and “drive2” refer to the x and y ball positions taken from the motor encoders, and “idle1” and “idle2” refer to the x and y ball positions taken from the passive roller encoders. Note that the motor encoders and passive roller encoders have opposite polarity, but are both meant to measure ball position. Ib θ¨b = τr − τs − τv − τc

(7)

Figure 6a shows ball velocity vs. time for the simulation using a model with no friction and using one with only static friction. Figure 6b shows ball velocity vs. time for the simulation using a model with all of the friction terms and one using static and viscous friction. Notice the large disparity of velocity magnitudes between these two figures. This disparity indicates that viscous friction is a major factor. Also notice in Figure 6b that coulomb friction has a significant effect on the ball velocity. The quasi-static assumption means that by ramping the torque slowly, the dynamic effects are negligible. Judging by the accelerations seen in Figure 5a-b (1-2 rad/s) and Ib , the left side of equation (7) is small (< 0.1 Nm) compared to the right side (> 5 Nm) making the quasi-static assumption reasonable. 7

8

15 model

model

drive1 6

drive1

drive2

drive2

10

idle1

idle1

idle2

4

idle2 ball velocity (rad/s)

ball velocity (rad/s)

5 2

0

0

−5 −2

−10

−4

−6

0

1

2

3

4

5 time (s)

6

7

8

9

−15

10

(a) Positive x Direction, Ramp = 100

0

1

2

3

4

5 time (s)

6

7

8

9

10

(b) Negative x Direction, Ramp = 100

20

5 model drive1

15

drive2

0

idle1 idle2 ball velocity (rad/s)


10

5

0

−5

−10 model

−5

drive1 drive2

−15

idle1

−10

idle2 −15

0

2

4

6 time (s)

8

10

−20

12

(c) Positive y Direction, Ramp = 100

0

1

2

3

4

5 time (s)

6

7

8

9

10

(d) Negative y Direction, Ramp = 100

Figure 5: Friction Test and Simulation Results

3500

8 all terms

all terms

static and viscous

3000

no coulomb

7

static no friction

6



2500

2000

1500

5

4

3

1000 2 500

0

1

0

1

2

3

4

5 time (s)

6

7

8

9

0

10

(a) Output of Limited Friction Models

0

1

2

3

4

5 time (s)

6

7

8

9

10

(b) Output of Better Friction Models

Figure 6: Comparison of Friction Models 8

2.1.3

Actuation Model

The actuation model is found in the “Control and Actuation/Actuation and Friction” block of Figure 4. Figure 7 is a block diagram of the Ballbot actuation system. An input is sent to the digital to analog converter (DAC) which outputs a voltage to the amplifier. The amplifier sends a current to the motor which outputs a torque to the roller. The roller then transmits a torque to the ball. In each block the input is multiplied by the number in the block to produce the output.

Figure 7: Actuation System Block Diagram Below are the DAC output voltage, VDAC , amplifier output current, Iout , and motor torque, τr , expressed in terms of the DAC input, CDAC . The DAC input is an integer in the range -2048 to 2047 and has units counts. 10 ∗ CDAC 10 ∗ CDAC = ≈ 0.00489 ∗ CDAC V (8) Cmax 2047 where the maximum magnitude DAC input, Cmax , is 2047. We have assumed that the amplifier operates in torque mode. According to page 23 of the amplifier spec sheet, the amplifier outputs a current, Iout , according to: VDAC =

10 ∗ CDAC ∗ 20 VDAC Ipeak A= A ≈ 0.00977 ∗ CDAC A (9) 10 2047 ∗ 10 where Ipeak =20A is the peak current limit. If the magnitude of Iout is greater than Ipeak , then Iout =Ipeak sign(Iout ). In the real amplifier (Copley Controls Model 412), if the continuous current limit is exceeded, a capacitor with a 1 second time constant at peak current begins to charge. When it is fully charged, the output current decays exponentially to the continuous current. The simulation models this as follows: If the magnitude of Iout is greater than Icont =10 A, the continuous current limit, an integral begins accumulating. If the integral is less than 14.427, then Iout =Iout , otherwise Iout =Icont sign(Iout ). The 14.427 was chosen so that the integral acts like the real circuit. Note that the peak and continuous current limits can be changed in the amplifier. Iout =

9

The torque constant, Kτ , for the motor is 0.133 Nm/A. To convert DAC input to motor torque, Nm ≈ 0.00130 ∗ CDAC Nm A which assuming no losses, is the torque applied to the roller τr . τr = Iout Kτ ≈ 0.00977 ∗ CDAC A ∗ 0.133

2.2

(10)

2D Ballbot with Arms Model

In order to design controllers we derive equations of motion for Ballbot. The dynamics of Ballbot with arms are derived using the Lagrangian formulation. Figure 8 shows a planar model of Ballbot. The planar equations of motion (and the corresponding planar controllers) are much simpler both to derive, understand, and implement than the 3D equations. Two perpendicular planar models can represent a 3D Ballbot accurately if Ballbot operates near the upright standing position where coupling effects between the two planes is small. The planar model is likely not as accurate in the case of arms that will operate out of the plane. Note that: • φ is measured with respect to the fixed world vertical, just as the IMU on the physical robot measures pitch and roll. • θ is measured with respect to moving φ, just as the encoders on the physical robot measure the position of the ball. • ψ is measured with respect to the moving φ, like the arm encoders would measure the arm angles on the physical robot.

a

cm

y

l

a

l

B

Figure 8: Planar Ballbot Model The position of the ball, pb , the position of the body, pB , and the position of the arm, pa are: 10



 rb (θ + φ)  pb =  0 0   rb (θ + φ) + lB sin φ  0 pB =  lB cos φ 

 rb (θ + φ) + la sin φ − acm (cos φ sin ψ + sin φ cos ψ)  0 pa =  la cos φ + acm (sin φ sin ψ + cos φ cos ψ)

The velocity of the ball, vb , the velocity of the body, vB , and the velocity of the arm, va are:   rb θ˙ + φ˙   vb =   0 0   rb θ˙ + φ˙ + lB φ˙ cos φ   vB =   0 ˙ −lB φ sin φ rb θ˙ + φ˙ + la φ˙ cos φ + acm φ˙ sin φ sin ψ − ψ˙ cos φ cos ψ − φ˙ cos φ cos ψ + ψ˙ sin φ sin ψ  0 va =   ˙ ˙ ˙ −la φ sin φ + acm φ cos φ sin ψ + ψ sin φ cos ψ + φ˙ sin φ cos ψ + ψ˙ cos φ sin ψ 

The kinetic energy of the ball, Tb , the kinetic energy of the body, TB , and the kinetic energy of the arm, Ta , are: 2 1 1 mb vb T vb + Ib θ˙ + φ˙ 2 2 1 1 mB vB T vB + IB φ˙ 2 = 2 2 2 1 1 = ma va T va + Ia ψ˙ + φ˙ 2 2

Tb = TB Ta

The potential energy of the ball, Vb , the potential energy of the body, VB , and the potential energy of the arm, Va , are: Vb = 0 VB = −mB glB cos φ Va = −mB g (la cos φ + acm (sin φ sin ψ − cos φ cos ψ)) The Lagrangian, L, is the total kinetic energy minus the total potential energy. 11

   

L=T −V The Lagrangian formulation dictates: d ∂L ∂L − = τ1 dt ∂ θ˙ ∂θ d ∂L ∂L = 0 − dt ∂ φ˙ ∂φ d ∂L ∂L = τ3 − dt ∂ ψ˙ ∂ψ

(11) (12) (13)

where τ1 is the torque applied to the ball by the rollers (including friction), and τ3 is the arm r symbolic package and rearranging torque. Evaluating equations (11-13) using the Matlab yields the following equation of motion.  ¨   ˙2  θ φ ¨    M φ + C ψ˙ 2  + G = T (14) ¨ ˙ ˙ ψ φψ where M is the 3x3 mass matrix with columns, M(:, 1), M(:, 2), and M(:, 3),   mrb2 + Ib M(:, 1) =  mrb2 + rb (cos φ (mB lB + ma la ) − ma acm cos (φ + ψ)) + Ib  −ma rb cma cos (φ + ψ) 

 mrb2 + rb (mB lB cos φ + ma la cos φ − ma acm cos (φ + ψ)) + Ib 2 + ma (la2 + a2cm ) + I  M(:, 2) =  mrb2 + 2rb (cos φ (mB lB + ma la ) − ma acm cos (φ + ψ)) + mB lB −ma rb cma cos (φ + ψ) − ma la acm cos ψ + ma a2cm + Ia   ma rb acm cos (φ + ψ) M(:, 3) =  −ma rb acm cos (φ + ψ) − ma la acm cos ψ + ma a2cm + Ia  ma c2ma + Ia m is the sum of the three masses,

m = mb + mb + ma I is the sum of the three inertias, I = Ib + IB + Ia C is the 3x3 matrix of coriolis and centrifugal terms with columns C(:, 1), C(:, 2), and C(:, 3),   −rb (sin φ (mB lB + ma la ) − ma acm sin (φ + ψ)) C(:, 1) =  −rb (sin φ (mB lB + ma la ) − ma acm sin (φ + ψ))  −ma la cma sin ψ 12



 ma rb acm sin (φ + ψ) C(:, 2) =  ma rb acm sin (φ + ψ) + ma la acm sin ψ  0   2ma rb acm sin (φ + ψ) C(:, 3) =  2ma rb acm sin (φ + ψ) + 2ma la acm sin ψ  0

G is the 3x1 gravity matrix, 

 0 G =  mB lB g sin φ + ma la g sin φ − ma acm g sin (φ + ψ)  −ma acm g sin (φ + ψ)

and U is the 3x1 matrix of applied torques.   τr − τs − τc − γv θ˙  0 U = ˙ τa − γva ψ

where γva is the coefficient of viscous friction for the arms. To apply linear control strategies we must linearize this system using the form of equation (15). dx˙ dx˙ x+ u (15) dx du h iT where the state vector is x = θ φ ψ θ˙ φ˙ ψ˙ , and the input vector is u = [τr τa ]T . We linearize the system of equation (14) about the equilibrium point θ = φ = ψ = θ˙ = φ˙ = ψ˙ = 0 to fit the system of equation (15). This corresponds to Ballbot standing upright up with its arms at its sides. x˙ =

3

Independent Balancing and Arm Controllers

The logical first step for implementing arms with Ballbot is to design an arm controller that works independently of an existing balancing/station keeping controller that uses only the rollers. To control the rollers we design a planar linear quadratic regulator (LQR)[1] h iT excluding the arm states. In this case the state vector is x = θ φ θ˙ φ˙ , and the input

vector is u = τr . LQR finds a state feedback law, u = −Kx that minimizes the following cost function near the linearization point. Z J= xT Qx + uT Ru dt

where Q is a 4x4 matrix of weights for the states, and R is the weight for the applied torque. The diagonal terms in Q correspond to the ball position, θ, the body angle, φ, the ball

13

˙ and the body angular velocity, φ. ˙ velocity, θ, follow are:  3000  0 Q=  0 0

The Q and R used to obtain the results that  0 0 0 30 0 0   0 30 0  0 0 30

and R = 1. These values were chosen only to achieve reasonable balancing (qualitatively), and not to achieve any specific quantitative performance goal. They are intended to give high importance to the ball position (first diagonal term) and low importance to the torque r command dlqr finds K for the given linear system of equation (15). In used. The Matlab order to make Ballbot move we define the augmented control law, u = −K (x − xp )

(16)

where xp is a vector of desired states that can change in time. In other words, if Ballbot is doing something other than station keeping (non-zero desired state), we must specify xp as a function of time. We choose the body tilt angle path, φp , and angular velocity path, φ˙ p to be zero at all times. We choose the ball position path, θp , to be a quintic polynomial in time as in equation (17) and ball velocity path, θ˙p , as the derivative of the polynomial as in equation (18). θp = At5 + Bt4 + Ct3 + Dt2 + Et + F

(17)

θ˙p = 5At4 + 4Bt3 + 3Ct2 + 2Dt + E

(18)

and

If one defines initial and final (or even a knot point in between) positions, velocities, and accelerations, one can solve for the coefficients, A, B, C, D, E, and F . The choice of a quintic polynomial is to ensure smooth accelerations and thus smooth torques. Sciavicco and Siciliano [8] suggest using a polynomial joint trajectory when no specific trajectory is desired between points. Eventually we will determine trajectories which respect Ballbot’s dynamic constraints (e.g. the body must lean forward and the ball must move backward before Ballbot moves forward while the quintic polynomial moves forward immediately). Equation (16) defines a planar controller. The real Ballbot and the SimMechanics simulation are both in 3D, so in practice we use two independent planar controllers. The first is for x ball movement, θx and θ˙x , and x body movement, namely pitch and pitch rate, φy and φ˙ y . The second is for y ball movement, θy and θ˙y , and y body movement, φx and φ˙ x . Two independent planar controllers will be most effective when the body is near its equilibrium point and coupling between x and y dynamics is small. Each arm degree of freedom (2 DoF for each arm) is controlled independently with a PD controller defined in equation (19) τa = Pψ (ψ − ψp ) + Dψ ψ˙ − ψ˙ p (19) 14

where Pψ and Dψ are the gains, and ψp and ψ˙ p are desired arm paths defined by quintic polynomials similar to equations (17) and (18). This controller requires solving the inverse kinematics problem for ψp and ψ˙ p given a goal point in space. The inverse kinematics problem is not solved here because this work deals only with simple movements (e.g. lifting the arms in the x direction).

4

Unified Balancing and Arm Controller

There might be some cases where coordinating roller and arm movement is desirable. One example is when lifting a heavy object causes the Ballbot to begin to lose balance. With the independent controllers of the previous sections, the arms might continue to lift and cause Ballbot to fall. With a unified controller, the arms might lower the heavy object to regain balance. h iT Again we turn to LQR using the full state vector x = θ φ ψ θ˙ φ˙ ψ˙ , and the input

vector u = [τr τa ]T and linearizing equation (14) about the zero state. This time Q is a 6x6 matrix of weights for the states and R is a 2x2 matrix of the weights for the applied torques. The Q and R used to obtain the results that follow are:   3000 0 0 0 0 0  0 30 0 0 0 0     0 0 3000 0 0 0    Q=  0 0 0 30 0 0    0 0 0 0 30 0  0 0 0 0 0 30

and

R=

1 0 0 1

This means that the relative importance of the ball position, θ, and the arm angle, ψ, is high and again the importance of the torque is low. When merely balancing, an LQR controller optimized to operate around the standing position with arms down is effective. In situations where Ballbot must move its arms (reach for something on a shelf, or carry a drink, for instance) this controller is not sufficient. First, LQR is a linear controller, and the sines and cosines in the gravity terms when the arms move away from the sides of Ballbot are nonlinear. Second, the gains for moving the arms are optimized for movement around the zero position where gravity has little effect. When lifting the arms, gravity works against the motion, and the LQR derived controller does not apply enough torque to move the arms very far. To make up for this, we introduce gravity compensation [8] for the arms. The arm torque, τa is a combination of the torque, τLQR , prescribed by the LQR gains (a feedback torque) and gravity (a feed forward torque). τa = τLQR + ma acm g sin ψ

15

(20)

Gravity compensation fixes the two problems mentioned above. It essentially takes away the nonlinear gravity terms and allows control about non-zero arm positions. This unified controller is also a planar controller, and in practice we use two planar controllers. In the previous section we assumed that the x and y dynamics are uncoupled when Ballbot is standing. Here the arms certainly have coupled dynamics in any out of plane movement. This controller is most effective when operating in the plane. Planar arm control is not a final solution. The unified controller of this section is intended to demonstrate the possible benefits of the unified approach vs. the independent roller and arm controller approach.

5

Performance

The purpose of these simulations was to: • Tune an arm controller independent of the balancing controller. For a number of simple movements (lifting the arms in various directions) can the controller follow the desired path? How much torque is required? • Observe the behavior of the balancing controller when the various arm motions are executed. Determine what problems exist with the balancing controller as a result of arm movement. • Compare the behaviors of the independent controllers and the unified controller. • Explore a few new control ideas. One SimMechanics output is a visualization window, shown in Figure 9. The red parts of the figure are what the visualization window actually shows. The black outline of the Ballbot’s body and the black dots representing the connection points of the arms are added for clarity. Red ellipsoids represent the bodies. The ellipsoids come from the inertia tensors of the bodies. The actual top of the Ballbot body is about 1.5 m whereas its ellipsoid does not extend that far. The arm ellipsoids extend further than the physical arms.

Figure 9: Example of SimMechanics Visualization Window

16

5.1

Arm Controller Performance

The basic design of the arms is an aluminum cylinder with a 1 kg hand. One test of the arm controller is to lift both arms up along Ballbot’s sides (the y direction) as seen in Figure 10a. This test was conducted using various hand masses to simulate what happens when Ballbot lifts objects. Note that lifting both arms in opposite directions will not affect Ballbot’s balance, allowing us to test the arm controller while Ballbot has perfect balance. The results in Figure 10 are for 5 kg hands. Figure 10b shows that the peak torque required for this action is 30.76 Nm. The peak torque required for 1 kg hands is 7.37 Nm. Figure 10c-d shows that this controller follows the desired path very well. Obviously the arms cannot supply an infinite amount of torque. Figure 11 shows results from a simulation where a 15 Nm torque limit was put on the arm motors. Figure 11a-b shows that when the motor saturates, the arms oscillate undamped (no friction is modeled) about a position less than the desired position. 35

X Y

Torque Arm 1 (Nm)

30 25 20 15 10 5 0 0

0.5

1

1.5

2

Time (s)

(a) Final Pose

(b) Torque vs. Time

2

1.5

1.5

Arm 1 Y Pos Actual Arm 1 Y Pos Planned Arm 2 Y Pos Actual Arm 2 Y Pos Planned

0.5

Velocity (radians/s)

Position (radians)

1

Arm 1 Y Vel Actual Arm 1 Y Vel Planned Arm 2 Y Vel Actual Arm 2 Y Vel Planned

1

0 −0.5

0.5

0

−0.5

−1 −1

−1.5 −2 0

0.5

1

1.5

2

Time (s)

−1.5 0

0.5

1

1.5

Time (s)

(c) Arm y Angle vs. Time

(d) Arm y Angular Velocity vs. Time

Figure 10: Both 5 kg Arms Raised in y Direction

17

2

2

1.5

1.5



0.5 0 −0.5

0.5

0

−0.5

−1 −1

−1.5 −2 0

1

2

3

4

5

−1.5 0

1

2

Time (s)

3

4

Time (s)

(a) Arm y Angle vs. Time

(b) Arm y Angular Velocity vs. Time

16 14

Torque Arm 1 (Nm)

Position (radians)

1


1

12

X Y

10 8 6 4 2 0 0

1

2

3

4

5

Time (s)

(c) Torque vs. Time Figure 11: Both 5 kg Arms Raised in y Direction with Saturated Torque

18

5

5.2

Balancing When Arms Move

Figure 12a shows the final pose of Ballbot raising one arm in the y direction. Note in Figure 12b-c that the ball moves to the right by about 6 cm and from Figure 12d-e that the body leans to the left by about 1.3 degrees. When Ballbot raises an arm, it settles into a leaning equilibrium that is different from the desired standing upright equilibrium where the body angle is zero. The arm movement causes an overall change in the center of mass which changes the equilibrium position. Figure 12f-g shows that the arm controller follows the planned path well. Figure 13a-f shows results of the same arm lifting simulation except with heavier 5 kg arms. This models what might happen when Ballbot picks up a load. In this case the station keeping/balancing controller fails. Figure 13a-b shows increasing ball oscillation and Figure 13c-d shows increasing body oscillation. As Ballbot loses its balance, the arm controller is not as effective as seen near the end of Figure 13e-f. The increased arm mass causes a larger change is the leaning equilibrium. Eventually the ball and body are far enough away from the desired standing upright equilibrium that the station keeping/balancing controller fights back. This causes the ball and body to oscillate between the leaning and standing upright equilibria and finally fall.

(a) Final Pose Figure 12: One 1 kg Arm Raised in y Direction

19

0.07 0.06

0.2

Actual Path Planned Path


0.15

Y Velocity (m/s)

Y Position (m)

0.05 0.04 0.03 0.02

0.1

0.05

0.01 0 0 −0.01 0

0.5

1

1.5

2

−0.05 0

2.5

0.5

1

Time (s)

(b) y Position vs. Time

Roll Rate (radians/s)

Roll (radians)


0.1

0.02

0.015


0.01

0.05

0

−0.05

0.005

0.5

1

1.5

2

−0.1 0

2.5

0.5

1

Time (s)

1.5

2

2.5

Time (s)

(d) Roll vs. Time

(e) Roll Rate vs. Time

1.6

1.6

1.4

1.4


1 0.8 0.6 0.4

1 0.8 0.6 0.4

0.2

0.2

0

0 0.5

1

1.5


1.2


1.2

Position (radians)

2.5

0.15

0.025

−0.2 0

2

(c) y Velocity vs. Time

0.03

0 0

1.5

Time (s)

2

2.5

Time (s)

−0.2 0

0.5

1

1.5

2

2.5

Time (s)

(f) Arm y Angle vs. Time

(g) Arm y Angular Velocity vs. Time

Figure 12: One 1 kg Arm Raised in y Direction 20

3 0.25 0.2


2


Y Velocity (m/s)

Y Position (m)

0.15 0.1 0.05 0 −0.05

1

0

−1

−0.1

−2 −0.15 −0.2 0

0.5

1

1.5

2

−3 0

2.5

0.5

1

1.5

2

2.5

Time (s)

Time (s)

(a) y Position vs. Time

(b) y Velocity vs. Time

3 0.25 0.2


2



Roll (radians)

0.15 0.1 0.05 0 −0.05

1

0

−1

−0.1

−2 −0.15 −0.2 0

0.5

1

1.5

2

−3 0

2.5

1.5

2

(c) Roll vs. Time

(d) Roll Rate vs. Time

2.5

6 5

1.6


1.2 1 0.8 0.6 0.4

3 2 1 0 −1

0.2

−2

0

−3 0.5

1

1.5


4


1.4

Position (radians)

1

Time (s)

1.8

−0.2 0

0.5

Time (s)

2

2.5

−4 0

0.5

1

1.5

2

2.5

Time (s)

Time (s)

(e) Arm y Angle vs. Time

(f) Arm y Angular Velocity vs. Time

Figure 13: One 5 kg Arm Raised in y Direction 21

Raising the arms in opposite x directions causes Ballbot to rotate about its vertical axis (a yaw). Figure 14a shows this pose in the xz plane. The xy plane view (from above) of Figure 14 makes the yawing apparent, as Ballbot began the simulation facing in the x direction. Keep in mind that no friction is modeled between the ground and ball or between the body and ball. In the real case where there is friction, there might not be any yawing, or at least there will be less yawing.

(a) Final Pose: xz View

(b) Final Pose: xy View

Figure 14: 5 kg Arms Raised in Opposite x-Directions Figure 15a-b is the final pose of Ballbot raising one arm in the x direction and out in the y direction. This causes both a yaw (Figure 15b) and the leaning equilibrium (Figure 15b-f and i-l) seen earlier. Note that the leaning is in both the x and y directions. The arm controller performs well (Figure 15g-h and m-n).

(a) Final Pose: 3D View

(b) Final Pose: xy View

Figure 15: 1 kg Arm Raised in x and y Directions

22

0.02

0.1


0.01 0.05

X Velocity (m/s)

X Position (m)

0 −0.01 −0.02 −0.03


0

−0.05

−0.04 −0.1 −0.05 −0.06 0

0.5

1

1.5

−0.15 0

2

0.5

Time (s)

(c) x Position vs. Time

2

0.15



0.1

Pitch Rate (radians/s)

0.02

Pitch (radians)

1.5

(d) x Velocity vs. Time

0.03 0.025

1

Time (s)

0.015 0.01 0.005

0.05

0

0

−0.05 −0.005 −0.01 0

0.5

1

1.5

−0.1 0

2

0.5

(e) Pitch vs. Time

2

1.6

1.4

1.4

Arm 1 X Pos Actual Arm 1 X Pos Planned Arm 2 X Pos Actual Arm 2 X Pos Planned

1 0.8 0.6 0.4

1 0.8 0.6 0.4

0.2

0.2

0

0 0.5

1

Arm 1 X Vel Actual Arm 1 X Vel Planned Arm 2 X Vel Actual Arm 2 X Vel Planned

1.2


1.2

Position (radians)

1.5

(f) Pitch Rate vs. Time

1.6

−0.2 0

1

Time (s)

Time (s)

1.5

2

Time (s)

−0.2 0

0.5

1

1.5

Time (s)

(g) x Arm Angle vs. Time

(h) x Arm Angular Velocity vs. Time

Figure 15: 1 kg Arm Raised in x and y Directions 23

2

0.4

0.06 0.05


0.2 0

Y Velocity (m/s)

Y Position (m)

0.04 0.03 0.02

−0.2 −0.4

0.01

−0.6

0

−0.8

−0.01 0

0.5

1

1.5

−1 0

2

Actual Path Planned Path 0.5

1

1.5

2

Time (s)

Time (s)

(i) y Position vs. Time

(j) y Velocity vs. Time 0.6

0.03 0.025


0.4


0.02

Roll (radians)

0.015 0.01 0.005 0 −0.005 −0.01 −0.015 −0.02 0


0.2 0 −0.2 −0.4 −0.6

1

1.5

−0.8 0

2

0.5

(k) Roll vs. Time

2

1.6

1.4

1.4


1 0.8 0.6 0.4

1 0.8 0.6 0.4

0.2

0.2

0

0 0.5

1


1.2


1.2

Position (radians)

1.5

(l) Roll Rate vs. Time

1.6

−0.2 0

1

Time (s)

Time (s)

1.5

2

Time (s)

−0.2 0

0.5

1

1.5

2

Time (s)

(m) y Arm Angle vs. Time

(n) y Arm Angular Velocity vs. Time

Figure 15: 1 kg Arm Raised in x and y Directions 24

Figure 16a-j shows results of Ballbot raising one arm in the y direction (as in Figure 12a) but starting off balance by 5 degrees of roll and pitch (rotations about x and y). Note the initial roll (Figure 16g) and pitch (Figure 16c). Ballbot recovers its balance nicely and establishes its leaning equilibrium. It finishes with non-zero x and y position and pitch and roll and zero x and y velocity and pitch rate and roll rate. The arm controller is initially a bit noisy (Figure 16j), but eventually follows the planned path closely. 1.4

0.16


0.12

1

0.1

0.8

0.08 0.06 0.04

0.6 0.4 0.2

0.02

0

0

−0.2

−0.02 0

0.5

1

1.5


1.2

X Velocity (m/s)

X Position (m)

0.14

−0.4 0

2

0.5

(a) x Position vs. Time

2

0.4 0.2


0.08

0


0.06

Pitch (radians)

1.5

(b) x Velocity vs. Time

0.1

0.04 0.02 0 −0.02

−0.2 −0.4 −0.6

−1 −1.2

−0.06

−1.4 0.5

1

1.5

2


−0.8

−0.04

−0.08 0

1

Time (s)

Time (s)

−1.6 0

0.5

1

1.5

Time (s)

Time (s)

(c) Pitch vs. Time

(d) Pitch Rate vs. Time

Figure 16: 1 kg Arms Raised in y Direction From Off Balance

25

2

0.5

0.02 0

0

Y Velocity (m/s)

Y Position (m)

−0.02 −0.04 −0.06 −0.08


−0.1

−0.5

Actual Path Planned Path −1

−0.12 −0.14 −0.16 0

0.5

1

1.5

−1.5 0

2

0.5

(e) y Position vs. Time

1.5

2

(f) y Velocity vs. Time

0.5

0.1


0.08

0


0.06

Roll (radians)

1

Time (s)

Time (s)

0.04 0.02 0 −0.02 −0.04


−0.5

−1

−1.5

−0.06 −0.08 0

0.5

1

1.5

−2 0

2

0.5

(g) Roll vs. Time

2

1.5

1.5


1


1

Position (radians)

1.5

(h) Roll Rate vs. Time

2

0.5 0 −0.5


−1 −1.5 −2 0

1

Time (s)

Time (s)

0.5

1

0.5

0

−0.5

−1

1.5

2

Time (s)

−1.5 0

0.5

1

1.5

Time (s)

(i) y Arm Angle vs. Time

(j) y Arm Angular Velocity vs. Time

Figure 16: 1 kg Arms Raised in y Direction From Off Balance 26

2

5.3

Compare Independent to Unified Control Approach

Figure 17a-b shows the final pose of Ballbot lifting its arms in the x direction using the independent controllers and the unified controller. The controllers show very similar performance (Figure 17c-j) in that Ballbot assumes the leaning equilibrium in both cases. With both control strategies the ball moves about 10 cm to the right and leans about 2.3 degrees to the left. We can see from Figure 17k-n that the independent arm controller follows the planned path slightly better than the unified controller does, while the unified controller requires slightly less torque (Figure 17o-p).

(a) Final Pose: Independent Controllers

(b) Final Pose: Unified Controller

0.1

0.08

0.12



0.1

X Position (m)

X Position (m)

0.08 0.06

0.04

0.02

0.06 0.04 0.02

0

−0.02 0

0

0.5

1

1.5

2

Time (s)

−0.02 0

0.5

1

1.5

2

Time (s)

(c) x Position vs. Time: Independent

(d) x Position vs. Time: Unified

Figure 17: Both 1kg Arms Raised in x-Direction: Compare Independent and Unified Control

27

0.5

0.2

0.4

0.15

0.3

X Velocity (m/s)

X Velocity (m/s)

0.25

0.1 0.05

0.2 0.1 0

0


−0.05 −0.1 0


0.5

1

1.5

−0.1 −0.2 0

2

0.5

0.01

0.01

0

0

−0.01

−0.01

−0.02


−0.03 −0.04

−0.05

−0.05

0.5

1

2

−0.02

−0.04

−0.06 0

1.5

(f) x Velocity vs. Time: Unified

Pitch (radians)

Pitch (radians)

(e) x Velocity vs. Time: Independent

−0.03

1

Time (s)

Time (s)

1.5

−0.06 0

2


0.5

Time (s)

1

1.5

2

Time (s)

(g) Pitch vs. Time: Independent

(h) Pitch vs. Time: Unified

0.2

0.05

0.1



0

−0.05

−0.1


0 −0.1 −0.2 −0.3

−0.15

−0.4 −0.2 0

0.5

1

1.5

2

−0.5 0


1

1.5

2

Time (s)

Time (s)

(i) Pitch Rate vs. Time: Independent

(j) Pitch Rate vs. Time: Unified

Figure 17: Both 1kg Arms Raised in x-Direction: Compare Independent and Unified Control 28

0

0.2

−0.2

−0.6


−0.2

Position (radians)

−0.4

Position (radians)

0


−0.8 −1

−0.4 −0.6 −0.8 −1 −1.2

−1.2 −1.4 −1.4 −1.6 0

−1.6 0.5

1

1.5

−1.8 0

2

0.5

Time (s)

0.2

0.4

0

0.2

−0.2

0

−0.4 −0.6 −0.8


−1.2 −1.4 −1.6 0

0.5

1

−0.4 −0.6 −0.8


−1 −1.2 −1.4

1.5

−1.6 0

2

0.5

1

0

0

−1

−1

−2

Torque Arm 1 (Nm)

Torque Arm 1 (Nm)

1.5

2

(n) x Arm Ang. Vel. vs. Time: Unified

1

X Y

−3 −4 −5

−2

−4 −5 −6

−7

−7 1.5

2

Time (s)

X Y

−3

−6

1

1

Time (s)

(m) x Arm Ang. Vel. vs. Time: Independent

0.5

2

−0.2

Time (s)

−8 0

1.5

(l) x Arm Angle vs. Time: Unified



(k) x Arm Angle vs Time: Independent

−1

1

Time (s)

−8 0

0.5

1

1.5

2

Time (s)

(o) Arm Torque vs. Time: Independent

(p) Arm Torque vs. Time: Unified

Figure 17: Both 1kg Arms Raised in x-Direction: Compare Independent and Unified Control 29

When executing the same x lifting motion with heavier 5 kg arms, the results are different. This time the independent controller (Figure 18a) fails almost immediately, and the unified controller (Figure 18b) balances for longer. The independent arm controller follows the planned path closely and uses more arm torque (Figure 18k, m, and o) while the unified controller deviates from the planned arm path and uses less arm torque (Figure 18l, n, and p) in order to stay balancing for longer. The unified controller has “decided” to abandon the goal of moving the arms to a desired position in favor of moving the arms to help balance. The next comparison is recovering from a 16 degree initial body pitch (Figure 19). It is obvious from Figure 19c-l that the unified controller is able to recover and the independent controllers are not. The independent arm controller attempts to keep the arms at Ballbot’s sides and uses small amounts of torque (Figure 19m, o, and q), while the unified controller swings the arms to help balance using more torque (Figure 19n, p, and r).

(a) Final Pose: Independent Controllers

(b) Final Pose: Unified Controller

0.5

0.7


0.4

0.5

X Position (m)

X Position (m)

0.3 0.2 0.1 0

0.4 0.3 0.2 0.1

−0.1 −0.2 0


0.6

0

0.5

1

1.5

2

2.5

3

Time (s)

−0.1 0

0.5

1

1.5

2

2.5

3

3.5

Time (s)

(c) x Position vs. Time: Independent

(d) x Position vs. Time: Unified

Figure 18: Both 5kg Arms Raised in x Direction: Compare Independent and Unified Control

30

2

1.5 1

1

X Velocity (m/s)

X Velocity (m/s)

0.5 0 −0.5 −1


0

−1


−2

−1.5

−3 −2 −2.5 0

0.5

1

1.5

2

2.5

−4 0

3

0.5

1

1.5

(e) x Velocity vs. Time: Independent

3

3.5

0.15



0.1 0.05

0.8

0

Pitch (radians)

Pitch (radians)

2.5

(f) x Velocity vs. Time: Unified

1.2 1

2

Time (s)

Time (s)

0.6 0.4

−0.05 −0.1 −0.15 −0.2

0.2

−0.25

0 −0.3

−0.2 0

0.5

1

1.5

2

2.5

−0.35 0

3

0.5

1

Time (s)

2.5

3

3.5

(h) Pitch vs. Time: Unified

3

3



2.5 2

2



2

Time (s)

(g) Pitch vs. Time: Independent

2.5

1.5

1.5 1 0.5

1.5 1 0.5 0 −0.5 −1

0 −1.5

−0.5 0

0.5

1

1.5

2

2.5

3

Time (s)

−2 0

0.5

1

1.5

2

2.5

3

3.5

Time (s)

(i) Pitch Rate vs. Time: Independent

(j) Pitch Rate vs. Time: Unified

Figure 18: Both 5kg Arms Raised in x Direction: Compare Independent and Unified Control 31

0

0.5

−0.2


−0.6


0

Position (radians)

Position (radians)

−0.4

−0.8 −1 −1.2

−0.5

−1

−1.5

−1.4 −2

−1.6 −1.8 0

0.5

1

1.5

2

2.5

−2.5 0

3

0.5

1

Time (s)


3.5

1


2



3

3

2

0 −1 −2 −3

1 0 −1 −2 −3

0.5

1

1.5

2

2.5

−4 0

3

0.5

1

Time (s)

1.5

2

2.5

3

3.5

Time (s)

(m) x Arm Ang. Vel. vs. Time: Independent

(n) x Arm Ang. Vel. vs. Time: Unified

50

5

X Y

40 30

0 −5

20

Torque Arm 1 (Nm)

Torque Arm 1 (Nm)

2.5

(l) x Arm Angle vs. Time: Unified

3

10 0 −10 −20

−10

−20 −25 −30

−40

−35

0.5

1

1.5

2

2.5

3

Time (s)

X Y

−15

−30

−50 0

2

Time (s)

(k) x Arm Angle vs. Time: Independent

−4 0

1.5

−40 0

0.5

1

1.5

2

2.5

3

3.5

Time (s)

(o) Arm Torque vs. Time: Independent

(p) Arm Torque vs. Time: Unified

Figure 18: Both 5kg Arms Raised in x Direction: Compare Independent and Unified Control 32

(a) Initial Pose: Independent Controllers

(b) Initial Pose: Unified Controller

(d) Final Pose: Unified Controller

1

0.8

0.8

0.7

0.6

0.6

0.4

0.5

X Position (m)

X Position (m)

(c) Final Pose: Independent Controllers

0.2 0 −0.2 −0.4

0.4 0.3 0.2 0.1

−0.6


−0.8 −1 0


0.2

0.4

0.6

0 0.8

1

1.2

1.4

1.6

−0.1 0

1

2

3

4

Time (s)

Time (s)

(e) x Position vs. Time: Independent

(f) x Position vs. Time: Unified

Figure 19: Extreme Balancing: Compare Independent and Unified Control

33

5

3

3


2


2

X Velocity (m/s)

X Velocity (m/s)

1 1

0

−1

0 −1 −2

−2

−3

−3 0

0.2

0.4

0.6

0.8

1

1.2

1.4

−4 0

1.6

1

2

(g) x Velocity vs. Time: Independent 0.3

0.2

0.2

0

5


0.1

Pitch (radians)

Pitch (radians)

4

(h) x Velocity vs. Time: Unified

0.4

−0.2 −0.4 −0.6

0 −0.1 −0.2

−0.8


−1 −1.2 0

3

Time (s)

Time (s)

0.2

0.4

0.6

−0.3

0.8

1

1.2

1.4

−0.4 0

1.6

1

2

3

4

5

Time (s)

Time (s)

(i) Pitch vs. Time: Independent

(j) Pitch vs. Time: Unified 4

0.5 0


−0.5



3

−1 −1.5 −2

2

1

0

−2.5

−3.5 0

−1


−3

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

−2 0

1

2

3

4

Time (s)

Time (s)

(k) Pitch Rate vs. Time: Independent

(l) Pitch Rate vs. Time: Unified

Figure 19: Extreme Balancing: Compare Independent and Unified Control 34

5

−3

x 10

1.2

Position (radians)

1


1.5

1


0.8

Position (radians)

2

0.5

0

0.6 0.4 0.2 0

−0.5

−0.2

−1 0

0.2

0.4

0.6

0.8

1

1.2

1.4

−0.4 0

1.6

1

2

3

4

5

Time (s)

Time (s)

(m) x Arm Angle vs. Time: Independent

(n) x Arm Angle vs. Time: Unified 3

0.06

2


0.02



0.04

0

−0.02

−0.04

1 0 −1


−2 −3

−0.06 0

0.2

0.4

0.6

0.8

1

1.2

1.4

−4 0

1.6

1

2

3

4

5

Time (s)

Time (s)

(o) x Arm Ang. Vel. vs. Time: Independent

(p) x Arm Ang. Vel. vs. Time: Unified 6

1

X Y

5 0.5

Torque Arm 1 (Nm)

Torque Arm 1 (Nm)

4 0

−0.5

−1

3 2 1 0

−1.5

−2 0

X Y 0.2

0.4

0.6

0.8

1

1.2

1.4

−1

1.6

−2 0

1

2

3

4

Time (s)

Time (s)

(q) Arm Torque vs. Time: Independent

(r) Arm Torque vs. Time: Unified

Figure 19: Extreme Balancing: Compare Independent and Unified Control 35

5

1.2

1.2

1

1

0.8

0.8

X Position (m)

X Position (m)

The next simulation is Ballbot waiting one second and then attempting to move 1 m in the x direction in the next 3 seconds (Figure 20a-n). The unified controller, which uses its arms to aid movement, gets to the goal slightly faster as seen in (Figure 20a-b). The independent arm controller, which is trying to keep its arms at Ballbot’s sides, has higher frequency (albeit very small) arm oscillations (Figure 20i, k, and m) while the unified controller operates more smoothly (Figure 20j, l, and n). As stated in section 4, the unified controller is two planar controllers, and its weakness is out of plane arm motion. Figure 21a-b shows that with an initial out of plane body angle (5 degrees of roll and pitch), the unified controller can still balance Ballbot. This balancing scenario does not require much arm motion. Excessive out of plane arm motion might cause Ballbot to fall.

0.6 0.4 0.2

0.4 0.2


0 −0.2 0

0.6

1

2

3

4


0 −0.2 0

5

1

2

Time (s)

(a) x Position vs. Time: Independent

4

5

(b) x Position vs. Time: Unified

1

1

0.8

0.8

0.6

0.6

X Velocity (m/s)

X Velocity (m/s)

3

Time (s)

0.4 0.2 0

0.4 0.2 0

−0.2


−0.4 0

1


−0.2

2

3

4

5

Time (s)

−0.4 0

1

2

3

4

Time (s)

(c) x Velocity vs. Time: Independent

(d) x Velocity vs. Time: Unified

Figure 20: Move 1m in 3 seconds: Compare Independent and Unified Control

36

5

0.1

0.1


0.08 0.06


0.08 0.06

Pitch (radians)

Pitch (radians)

0.04 0.02 0 −0.02

0.04 0.02 0

−0.04 −0.02 −0.06 −0.04

−0.08 −0.1 0

1

2

3

4

−0.06 0

5

1

2

Time (s)

(e) Pitch vs. Time: Independent

4

5

(f) Pitch vs. Time: Unified

0.5

0.3


0.4

0.2

0.3



3

Time (s)

0.2 0.1 0 −0.1

0.1 0 −0.1 −0.2


−0.3

−0.2 0

1

2

3

4

−0.4 0

5

1

2

Time (s)

3

4

5

Time (s)

(g) Pitch Rate vs. Time: Independent

(h) Pitch Rate vs. Time: Unified

−4

x 10

0.15

1.5


Position (radians)

1 0.5


0.1

Position (radians)

2

0 −0.5

0.05

0

−1 −0.05

−1.5 −2 0

1

2

3

4

5

Time (s)

−0.1 0

1

2

3

4

Time (s)

(i) x Arm Angle vs. Time: Independent

(j) x Arm Angle vs. Time: Unified

Figure 20: Move 1m in 3 seconds: Compare Independent and Unified Control 37

5

−3

x 10

0.3

3



2 1

0.2


4

0 −1 −2

0.1 0 −0.1


−0.2 −0.3

−3 −4 0

1

2

3

4

−0.4 0

5

1

2

Time (s)

3

4

5

Time (s)

(k) x Arm Ang. Vel. vs. Time: Independent

(l) x Arm Ang. Vel. vs. Time: Unified 5

0.2

4 0.15

3

Torque Arm 1 (Nm)

Torque Arm 1 (Nm)

0.1 0.05 0 −0.05

2 1 0 −1

−0.1

X Y

−0.15 −0.2 0

X Y

1

2

3

4

−2

5

−3 0

1

2

3

4

5

Time (s)

Time (s)

(m) Arm Torque vs. Time: Independent

(n) Arm Torque vs. Time: Unified

Figure 20: Move 1m in 3 seconds: Compare Independent and Unified Control

(a) Initial Pose

(b) Final Pose

Figure 21: Unified Controller Balancing Out of Plane 38

5.4

New Ideas

Two other means of controlling Ballbot were briefly explored. The first is using only the arms to balance starting from a 1 degree body pitch. Figure 22a-g shows that Ballbot eventually falls down with this specific arm-only controller. Note in Figure 22c-d that Ballbot does reverse its pitch before going unstable. This requires a large initial torque (Figure 22g) of over 100 Nm. This large torque makes balancing using only arms unpractical. The second new idea is inspired by the Segway personal mobility device (www.segway.com) and the Robotic Mobility Platform [7]. These devices balance in 2D and move by leaning forward. The lean causes the controller to accelerate to regain the robot’s balance. It is possible that Ballbot can move by inducing the same body lean by moving its arms. Here we control Ballbot’s forward motion by defining a ball path and applying an arm torque proportional to the planned ball velocity while damping the arm motion so that the arms do not swing out of control. Figure 23a-d show that this is a feasible approach, but at least in this initial trial, this strategy does not follow the planned path as closely as the controllers described earlier (see Figure 20). 8

0.6


0.5


6 4

X Velocity (m/s)

X Position (m)

0.4 0.3 0.2 0.1

0 −2 −4

0 −0.1 0

2

−6

0.2

0.4

0.6

0.8

1

1.2

−8 0

1.4

0.2

0.4

0.6

0.8

1

Time (s)

Time (s)



Figure 22: Balancing With Only Arms

39

1.2

1.4

0.1

15

0

10


Pitch (radians)

−0.1


−0.2 −0.3 −0.4

5

0

−5

−10

−0.5 −0.6 0


0.2

0.4

0.6

0.8

1

1.2

−15 0

1.4

0.2

0.4

0.6

(c) Pitch vs. Time

1.2

1.4

1.2

1.4

20

3


1

0 −20


2

0 −1

−40 −60 −80

−2

−100

−3

−120

0.2

0.4

0.6

0.8

1

1.2


−140 0

1.4

0.2

0.4

0.6

Time (s)

0.8

1

Time (s)

(e) Arm x Angle vs. Time

(f) Arm x Ang. Vel. vs. Time

800

X Y

600 400

Torque Arm 1 (Nm)

Position (radians)

1

(d) Pitch Rate vs. Time

4

−4 0

0.8

Time (s)

Time (s)

200 0 −200 −400 −600 −800 −1000 0

0.2

0.4

0.6

0.8

1

1.2

1.4

Time (s)

(g) Arm Torque vs. Time Figure 22: Balancing With Only Arms

40

1.2

1.2

1

1 0.8


0.6

X Velocity (m/s)

X Position (m)

0.8

0.4

0.6 0.4

0.2

0.2

0

0

−0.2 0


2

4

6

8

−0.2 0

10

2

4

Time (s)


10

3

0.6

2.5


0.2


2


0.4

Position (radians)

8


0.8

0 −0.2 −0.4

1.5 1 0.5 0 −0.5

−0.6 −0.8 0

6

Time (s)

−1 2

4

6

8

−1.5 0

10

2

4

6

8

10

Time (s)

Time (s)

(c) Arm x Angle vs. Time

(d) Arm x Angular Velocity. vs. Time

Figure 23: Driving Body Movement With Arms

6

Conclusions and Future Work

This work examined the dynamics of a dynamically stable mobile robot with arms and explored two control strategies. The first control strategy uses independent controllers for balancing/station keeping (ball and body feedback to drive the rollers) and for the arms (arm feedback to drive the arms). The second controller is a unified controller with full state feedback (ball, body, and arms) to drive the rollers and arms. From the results of simulations, we make these conclusions: • When working independently of the balancing/station keeping controller, the PD arm controller followed simple planned arm trajectories very well. The exception is in extreme cases when Ballbot is falling. • For both the independent controllers and the unified controller, Ballbot assumes a leaning equilibrium when the arms move. This moves Ballbot’s center of mass.

41

• Both control strategies try to drive Ballbot to a standing equilibrium. When Ballbot carries a heavy load, the leaning equilibrium is significantly different than the standing equilibrium. In this case, the independent controllers cannot stabilize Ballbot because the controller tries to keep the arms up. The unified controller also fails in this circumstance, but unified balancing/arm control in general might prove to be successful in handling heavy loads. • In cases where there is no planned arm motion (simply balancing or simply moving) the unified controller outperforms the independent controllers. This is because the arm controller uses it arms to help balance or move Ballbot’s body. • The unified arm controller uses gravity compensation. This strategy only works when the gravity forces are known. If Ballbot carries an object of unknown mass, Ballbot does not know how to compensate for its weight. Another problem of unknown objects is possible saturation of the arm motors which causes arm oscillations. • The SimMechanics simulation does not handle yaw well because of a software quirk in specifying constraints. To overcome some of the problems mentioned the following work is suggested: • Because of the conflict between a leaning equilibrium and the standing equilibrium new control strategies should be explored. Calculating the natural equilibrium point online and setting that as the control goal is an obvious idea to try. Research in humanoid balancing provides a wealth of resources including momentum methods [10] and zero moment point methods [3]. • Instead of using LQR to balance Ballbot while arms act independently as disturbances, treat the arms as disturbances and try a disturbance rejection strategy such as H∞ [2]. • Explore other unified control strategies or a hybrid strategy that uses independent controllers in some cases and a unified controller in other cases. • Explore adaptive controllers or online load estimation to compensate for heavy or unknown loads. These tasks need to be done to advance Ballbot in general: • First and foremost, develop an interface/operating environment like RHexLib to make Ballbot testing easy. • Solve the yaw problem in SimMechanics or use other software that has a more flexible way of defining constraints. • The controllers of this paper are planar. This is because of the complexity of Ballbot’s 3D dynamics. 3D controllers must be developed for the arms especially. The ball and body control problem also becomes a 3D problem when the body is away from the standing equilibrium as it is when carrying heavy loads. 42

• Develop better trajectory plans for both the ball/body and the arms. The quintic polynomial is smooth, but it does not always plan things that are physically possible. For example, Ballbot cannot begin moving forward immediately. It must develop a forward lean first which causes it to move backward. • Develop better contact models. The friction model here lumps all friction into friction between the ball and rollers. There is also contact between the ground and ball and between the body and ball. A good starting reference is Montana [6]. • Determine the parameters of the system experimentally. Some of the parameters were measured directly (body and ball mass and center of mass) and others are taken from data sheets (the entire actuation system like the motor torque constant). Run tests to tune or even optimize the physical parameters so that the simulation fits the real Ballbot test data.

7

References

˙ om, K.J. and Wittenmark, B. Computer-Controlled Systems: Theory and Design. [1] Astr¨ 3rd Ed., Prentice Hall, Upper Saddle River, NJ, 1997. [2] Chen, B. H∞ Control and Its Applications, Springer, London, 1998. [3] Erbatur, K., Obazaki, A., Obiya, K., Takahashi, T., and Kawamura, A. ”A Study on the Zero Moment Point Measurement for Biped Walking Robots”, Proc. of the 7th International Workshop on Advanced Motion Control, 3-5 July, 2002, Pages 431-436. [4] Lauwers, T.B., Kantor, G.A., and Hollis, R.L. ”One is Enough!” Proc. of 2005 International Symposium of Robotics Research, October 12-15, 2005. [5] Lauwers, T.B., Kantor, G.A., and Hollis, R.L. ”A Dynamically Stable Single-Wheeled Mobile Robot With Inverse Mouse-Ball Drive”. Proc. of 2006 IEEE International Conference on Robotics and Automation, May 15-19, 2006, Pages:2884 - 2889. [6] Montana, D.J., ”The Kinematics of Contact and Grasp” International Journal of Robotics Research, Vol. 7, No. 3, June, 1988, pages 17-32. [7] Nguyen, J., Morrell, J., Mullens, A., Burnmeister, S., Farrington, K., Thomas, K., and Gage, D. ”Segway Robotic Mobility Platform”. Proc. of SPIE - Volume 5609: Mobile Robots XVII, October, 2004. [8] Sciavicco, L. and Siciliano, B. Modelling and Control of Robot Manipulators. 2nd Ed., Springer, London, 2000. [9] Thibodeau, B.J., Deegan, P., and Grupen, R. ”Static Analysis of Contact Forces With a Mobile Manipulator”. Proc. of 2006 IEEE International Conference on Robotics and Automation, May 15-19, 2006, Pages:4007 - 4012. [10] Yoshida, E., Guan, Y., Sian, N.E., Hugel, V., Blazevic, P., Kheddar, A., and Yokoi, K. ”Motion Planning for Whole Body Tasks by Humanoid Robots”, Proc. of 2005 IEEE International Conference on Mechatronics and Automation, July, 2005, Pages 1784-1789.

43

A

SimMechanics Block Diagrams

A.1

Independent Controllers

Figure 24 is a lower level SimMechanics block diagram of the “Control and Actuation” block in Figure 4. Starting from the left side of the figure, one sees the “X Sensor” block which senses the angular position and velocity of the revolute joint connecting the “Body” and the “x1 roller” in Figure 4. The range of the angular position measurement is 0 to 2π, so it is converted to a continuous angle in the next block. Below the “X Sensor” is the “Body Sensor” block which outputs the orientation of the body in the form of a quaternion and quaternion derivative. The next block converts the quaternion to pitch, pitch rate, roll, and roll rate. Below the “Body Sensor” is the “Path” block. This block takes a distance, the desired number of seconds in which to move that distance, an amount of time to wait before moving, and a direction and creates a path according to equations (17) and (18). Below the “Path” block is the “Y Sensor” which measures the angular position and velocity of the joint connecting the “Body” and the “y1 roller” in Figure 4. Moving to the right, the augmented (continuous angles and quaternion changed to pitch and roll) outputs of these three sensor blocks and the “Path” block output are input to the “Controller”. The “Controller” outputs a signal to the “Actuation and Friction” block, which in turn sends torques (after being divided by two by the two gain blocks to split the overall torque between the two motors) to the four roller actuation blocks, “x1 Act”, “x2 Act”, “y1 Act”, and “y2 Act”. ap 3 Sense X

Angle Continuous Angle Rate

av

X Continuous Angle1

X Sensor

pitch

pitchrate

2 Sense Body

u

X Ball/Body

Y DAC

7 Act Y2

fcn Y2 Act

roll

Body Sensor Y Dac

Y Vel

rollrate

Torque Y

.5

6 Act Y1

X Vel

Gain

Y1 Act

DAC Y Torque X

xpath

1

dist

distance 1

3

move

move time

dead

.5

5 Act X1

DAC X

Gain1

xvelpath

X Dac

pitchpath pitchratepath

Y Ball/Body

X1 Act

Actuation and Friction

X DAC

4 Act X2

fcn wait time 0

0

x direction

ypath

xdir

X2 Act yvelpath

ydir rollpath

y direction

time

rollratepath

Clock Path Controller

ap 1 Sense Y

av Y Sensor


Y Continuous Angle

Figure 24: Ball Control and Actuation Block for Independent Controllers Model Figure 25 is a lower level SimMechanics block diagram of the “Controller Block” in Figure 24. It takes all of the sensor data described above and inputs them into two identical controllers - one in the x direction and one in the y direction Moving from left to right, each controller takes the differences between roller position, roller velocity, body angle (either roll 44

or pitch), and body velocity and their respective paths (all measured in radians). These differences are multiplied by gains and then added together and multiplied by negative one. This results in a torque (in Nm) to be applied to the ball. The ball torque is first converted to a roller torque and then to a DAC input.

xvel −K− To Workspace6 X Ball Torque X Ball/Body to Roller Torque1 Cont

xpos

xcont

To Workspace4 To Workspace8 −3.1 P_PosX X Pos −2.5 X Dac D_PosX −K− X Vel 1 X Ball/Body

−K−

2 X DAC

X Ball Torque X Roller Torque to Roller Torque to DAC

−1212.6 P_AngleX

xdac

Pitch To Workspace3 −374.1 D_AngleX Pitch Rate

Subtract

pitch To Workspace10

pitchrate To Workspace11 rollrate

roll

To Workspace12

To Workspace9 374.1 D_AngleY Roll Rate 1212.6

Y Dac

P_AngleY −K− Roll 2 Y Ball/Body

Y Ball Torque to Roller Torque

−2.5

−K−

1 Y DAC

Y Roller Torque to DAC

D_PosY

ydac

Y Vel

To Workspace2 −3.1 P_PosY

Y Pos

Subtract1 ycont

ypos To Workspace5

−K−

To Workspace1

yvel To Workspace7

Y Ball Torque Y Ball/Body to Roller Torque1 Cont

Figure 25: Ball Controller Block for Independent Controllers Model Figure 26 is a lower level SimMechanics block diagram of the “Actuation and Friction” block in Figure 24. This block contains the actuation model summarized by Figure 7. The blocks “Handle X Friction” and “Handle Y Friction” contain Matlab code that implements 45

the friction model described in Section 2.1.2.

1 Y Vel

current

3 DAC Y

10/2047 Y DAC Saturation

fcn i_out

1 s

2

DAC to VoltVolt to Current

Peak Current Limit

10/2047

Integrator 10

X DAC Saturation

DAC to Volt1 Volt to Current1 Peak Current Limit1

10

Handle Y Cont Current Limit

0 1 Torque Y

fcn ty_out ty_in

Motor Torque Constant (Nm/Amp)

Integrator1

fcn i_out

ytorque

Handle Y Friction

To Workspace1 2 X Vel

current

1 s

2

yvel_in

integral

Cont Current Limit 4 DAC X

0.133

0.133

xvel_in

fcn tx_out

0 2 Torque X

tx_in

integral

Handle X Cont Current Limit

Motor Torque Constant1 (Nm/Amp)

Handle X Friction

Cont Current Limit1

xtorque To Workspace4

Figure 26: Actuation and Friction Block for Both Models Figure 27 is a lower level SimMechanics block diagram of the “Arm Controller” block in Figure 4. At the bottom are four blocks with light blue outlines. These are the arm sensors. They sense the angular position and velocity of the x and y rotations of each arm. Also at the bottom is the “Path” block which is similar to the “Path” block in Figure 24. The outputs of the arm sensors and the “Path” block are sent to the “Controller” block. The “Controller” outputs torques (in Nm) which are compared to torque limits in the “Saturation” blocks. The outputs of the “Saturation” blocks are sent to the four arm actuation blocks, “Arm1x Torque”, “Arm1y Torque”, “Arm2x Torque”, and “Arm2y Torque”. These four blocks apply torques to the arms.

46

47

Figure 27: Arm Control and Actuation Block for Independent Controllers Model

arm2xvel To Workspace5 arm2xpos arm1xvel

To Workspace7

To Workspace4 arm1xpos arm1xtorque

To Workspace6

ap

To Workspace8

av

3 Sense Arm1

Arm1 XPos

Arm1x

Act Arm1x Y1 Torque

Arm1 XVel ap Arm2 XPos

av

Saturation

X Arms

Arm1x Torque

Arm2x Arm2 XVel

arm1_xpath arm1_xdist arm1_xvelpath

0 arm dist −C−

X1 Torque

Saturation1

arm1_ydist arm2_xpath

arm dist1

0

Mechanical Branching Bar2

1 Act Arm1

Arm1y torque

arm2_xvelpath arm2_xdist

fcn

arm dist2 −C−

arm1_ypath arm2_ydist

2

arm1_yvelpath arm_move arm2_ypath

arm move time

arm_time arm2_yvelpath

arm dist3

Clock1

Act Arm1y

X2 Torque

arm1ytorque To Workspace9

Path1 Y Arms

time

arm2xtorque

ap To Workspace12

av

To Workspace10

Arm1 YPos

Y2 Torque2

Arm1y

Act Arm2x

Arm1 YVel 4 Sense Arm2

ap av

Controller

Arm2 YPOS

Saturation2

Arm2y Arm2 YVel

Arm2x Torque Mechanical Branching Bar3

arm2yvel arm2ypos

To Workspace

Saturation3

Arm2y Torque

To Workspace1 arm1yvel arm1ypos

To Workspace2

Act Arm2y

arm2ytorque

To Workspace3 To Workspace11

2 Act Arm2

Figure 28 is a lower level SimMechanics block diagram of the “Controller Block” in Figure 27. It shows four identical PD controllers (2 DoFs for each arm). The differences between the arm angular position and velocity and their respective paths are multiplied by gains and then multiplied by negative one. The controllers output torques in Nm.

Figure 28: Arm Controller Block for Independent Controllers Model

48

A.2

Unified Controller

Figure 29 is very similar to Figure 24 except that it is the “Control and Actuation” block for the model that uses the unified balancing/station keeping and arm controller. The difference is the addition of arm sensing blocks and an arm path block in the lower left corner. The “Actuation and Friction” block is identical to the block for the independent controllers shown in Figure 26

ap 4 Sense X


av

X Continuous Angle1

X Sensor

8 Act Y2 Y2 Act X Ball/Body

pitch Y Vel Torque Y

.5 Gain

5 Sense Body

u

2 Act Y1

X Vel

Y DAC

pitchrate

fcn

Y1 Act

DAC Y roll

Body Sensor

Torque X

Y Ball/Body

.5

3 Act X1

DAC X

Gain1 rollrate

Y Dac

X1 Act

Actuation and Friction 9 Act X2

xpath

1

dist

X Dac

X Arms

X2 Act

xvelpath

distance 1

3

move

move time

dead

pitchpath X DAC

pitchratepath

fcn wait time 0

0

ypath

xdir

x direction

yvelpath ydir

Y Arms

rollpath

y direction

time

rollratepath

Clock Path

ap 1 Sense Y

Controller


av

Y Continuous Angle

Y Sensor

ap 6 Sense Arm1

av Arm1x

ap −C− arm dist 2 arm move time

arm_dist

arm_xpath

av Arm2x

arm_xvelpath arm_movefcn arm_ypath

arm_timearm_yvelpath

ap av

Clock1 Path1

7 Sense Arm2

Arm1y

ap av Arm2y

Figure 29: Ball Control and Actuation Block for Unified Controller Model Figure 30 is a lower level SimMechanics block diagram of the “Controller Block” in Figure 29. It takes all of the sensor data from the rollers, body, and arms and inputs them into two 49

identical controllers - one in the x direction and one in the y direction. Moving from left to right, each controller takes the differences between roller position, roller velocity, body angle (either roll or pitch), body velocity, arm 1 angular position and velocity, and arm 2 angular position and velocity and their respective paths (all measured in radians). These differences are multiplied by gains and then added together and multiplied by negative one. There are eight inputs to each controller. The controllers output torques (in Nm) to be applied to the ball. The ball torque is first converted to a roller torque and then to a DAC input. −3.1 P_PosX xrollcont −2.5 D_PosX 1 X Ball/Body

To Workspace4

−K− X Ball Torque X Ball/Body to Roller Torque1 Cont

xdac −1215.6

To Workspace5

P_AngleX

−373.7 X Dac D_AngleX −K−

−K−

X Ball Torque to Roller Torque

2 X DAC

X Roller Torque to DAC

24.3 P_XA1

7.2 D_XA1 3 X Arms

Subtract

24.3 −K− P_XA2 X Ball Torque to Roller Torque2

X Arm Cont

7.2 xrollarmcont D_XA2 To Workspace1

373.7 yrollcont D_AngleY To Workspace2 −K− 1215.6 Y Ball Torque to Roller Torque2

Y Ball/Body Cont

P_AngleY 2 Y Ball/Body

ydac −2.5

To Workspace6

D_PosY

Y Dac

−3.1 P_PosY −K−

−K−

Y Ball Torque to Roller Torque

24.3

1 Y DAC

Y Roller Torque to DAC

P_YA1

7.2 D_YA1 4 Y Arms

Subtract1

24.3 −K−

P_YA2

Y Ball Torque to Roller Torque1

Y Arm Cont

7.2 yrollarmcont D_YA2 To Workspace3

Figure 30: Ball Controller Block for Unified Controller Model Figure 31 is very similar to Figure 27 except that it is the “Arm Controller” block for the model that uses the unified balancing/station keeping and arm controller. One difference is the addition of arm sensing blocks and an arm path block in the lower left corner. The other difference is the addition of four “gravity compensation” blocks (Matlab code inside). 50

These blocks use equation (20). ap 4 Sense X


av

X Continuous Angle1

X Sensor

xpos

arm1xtorque

To Workspace5

To Workspace18

XPos

X Ball/BodyY1 Torque

XVel

pitch

xvel

u

Act Arm1x

torque

fcn Gain1

gtorque

psi

To Workspace1

Pitch

pitchrate

5 Sense Body

0.5

Y1 gravity compensation

Saturation

fcn Pitch Rate

roll

Body Sensor

Y Ball/BodyX1 Torque

0.5 Gain

Mechanical Branching Bar2

torque

fcn pitch rollrate

Arm1x Torque

gtorque

Saturation1

psi

To Workspace4

Arm1y torque

x gravity compensation1

arm1ytorque

pitchrate

To Workspace17

To Workspace7

xpath

1

rollrate

dist

X Arms

X2 Torque

0.5

torque

Gain2

psi

1

3

move

move time

dead

To Workspace8 pitchpath

roll

0

0

Y Arms

ydir

Y2 Torque

Roll Rate

rollpath time

psi

fcn

gtorque

Saturation2

Arm2x Torque Mechanical Branching Bar3

Controller YVel

Saturation3 ypos


av

torque

Gain3


Roll

Path

ap

0.5

rollratepath

Clock

3 Sense Y

arm2xtorque To Workspace19

yvelpath

x direction

Act Arm2x

To Workspace6 ypath

xdir

y direction

gtorque


pitchratepath

fcn wait time

Act Arm1y fcn

xvelpath

distance

1 Act Arm1

2 Act Arm2

Arm2y Torque

To Workspace2

YPos

Y Continuous Angle

Y Sensor

Act Arm2y

yvel To Workspace3

time arm1xvel

arm2ytorque

To Workspace21

arm1xpos

To Workspace11

To Workspace20

To Workspace9 ap av

6 Sense Arm1

Arm1 XPos

Arm1x

Arm1 XVel ap Arm2 XPos

av Arm2x

arm dist

Arm2 XVel

arm1_xpath arm1_xdist arm1_xvelpath

−C− 0

arm2xpos

arm1_ydist arm2_xpath

arm2xvel

arm dist1 −C−

To Workspace10

arm2_xvelpath arm2_xdist

fcn

arm dist2

arm1_ypath arm2_ydist

0 2

arm1_yvelpath arm_move arm2_ypath

arm move time

arm_time arm2_yvelpath

arm dist3

Clock1

To Workspace12

arm1ypos To Workspace13

Path1 time1

ap

To Workspace22

av

arm1yvel Arm1 YPos

To Workspace15

Arm1y Arm1 YVel 7 Sense Arm2

arm2ypos

ap av

To Workspace14

Arm2 YPOS

Arm2y Arm2 YVel

arm2yvel To Workspace16

Figure 31: Arm Control and Actuation Block for Unified Controller Model Figure 32 is a lower level SimMechanics block diagram of the “Controller Block” in Figure 31. There are four identical controllers - one for each DoF of each arm. Moving from left to right, each controller takes the differences between roller position, roller velocity, body angle (either roll or pitch), body velocity, the arm angular position of the DoF being controlled and the arm angular velocity of the DoF being controlled, and their respective paths (all measured in radians). These differences are multiplied by gains and then added together and multiplied by negative one. There are six inputs to each controller. The controllers output torques (in Nm) to be applied to the arms.

51

−0.2

xarmcont

P_PosX

To Workspace19

−0.1

X Ball/Body Cont

D_PosX 1 X Ball/Body

X1 Dac −37.6 2 X1 Torque

P_AngleX

−11.5 D_AngleX

Subtract

X Arm1 Cont

20.8 P_XA1

xarmarm1cont X2 Dac

5.2

Subtract3

To Workspace2

D_XA1 3 X Arms

3 X2 Torque

X Arm2 Cont 20.8

xarmarm2cont To Workspace4

P_XA2

5.2 D_XA2

yarmcont

Subtract2

To Workspace1

11.5

Y Ball/Body Cont

D_AngleY

37.6 P_AngleY Y1 Dac 2 Y Ball/Body

1 Y1 Torque

−0.1 D_PosY

−0.2 Subtract1

P_PosY

Y Arm 1 Cont

20.8 P_YA1

yarmarm1cont To Workspace3

5.2 Y2 Dac1 D_YA1 Subtract4 4 Y Arms

4 Y2 Torque

20.8 Y Arm 2 Cont P_YA2

5.2 D_YA2

yarmarm2cont Subtract5

To Workspace5

Figure 32: Arm Controller Block for Unified Controller Model

52

Modeling Dynamics and Exploring Control of a Single-Wheeled

Modeling Dynamics and Exploring Control of a Single-Wheeled

Suggest Documents

Dynamics Modeling and Periodic Control of ...

Unified Modeling Approach of Kinematics, Dynamics and Control of a

Dynamics modeling and control of a transport aircraft ...

Modeling, Dynamics and Control of Chemical and Biological Systems

Modeling the Dynamics and Control of Transmission of Schistosoma ...

Modeling the transmission dynamics and control of rabies in China

pdf-1451\system-dynamics-modeling-simulation-and-control-of ...

DYNAMICS AND CONTROL OF A CLASS OF

DYNAMICS AND CONTROL OF A GYROSCOPE-STABILIZED ...

Modeling and Control of a Balloon Borne

Modeling and Predictive Control of a Cash

Modeling and Control of a Balloon Borne

Modeling, Control, and Numerical Simulations of a

Modeling, Identification and Control of a

Modeling and Control of a Battery Connected

Modeling and Control of a Battery Connected

Modeling Illicit Drug Use Dynamics and Its Optimal Control Analysis

[PDF] Download System Dynamics: Modeling, Simulation, and Control ...

Kane Method Based Dynamics Modeling and Control Study for Space ...

Hybrid Systems and Control With Fractional Dynamics (I): Modeling

[PDF] Process Dynamics, Modeling, and Control Read ... - Google Sites

Read Book Process Dynamics, Modeling, and Control ... - Google Sites

Dynamics of insecticide resistance and exploring ...

Control-Oriented Modeling and Robust Control of a ... - Conferences