Dynamic Constraint-Based Optimal Shape Trajectory Planner for Shape-Accelerated Underactuated Balancing Systems

Home , Inverted pendulum

2010 Robotics: Science and Systems June 27-31, 2010, Zaragoza, Spain Dynamic Constraint-based Optimal Shape Trajectory Planner for Shape-Accelerated Underactuated Balancing Systems

Umashankar Nagarajan The Robotics Institute Carnegie Mellon University Pittsburgh, PA 15213 Email: [email protected]

Abstract— This paper presents an optimal shape trajectory robots, like Segway, have kinematic constraints restricting planner for shape-accelerated underactuated balancing systems, their direction of motion, whereas, single spherical-wheeled which are destabilized by gravitational forces. These systems balancing robots, like ballbots, have omnidirectional motion have unactuated shape variables and fully actuated external variables. They also have the same number of actuated and that make them more suitable for operation in constrained unactuated degrees of freedom. Their equations of motion result spaces. In this paper, such omnidirectional balancing systems in nonholonomic acceleration/dynamic constraints, which relate are referred to as shape-accelerated underactuated balancing the acceleration of external variables to the position, velocity and systems (described in Sec. II-B). acceleration of shape variables. This paper describes a procedure An interesting and troubling factor in control and plan- to use the dynamic constraints for planning shape trajectories, which when tracked will result in optimal tracking of desired ning of such underactuated systems is the constraint on external configuration trajectories. Examples of planned optimal their dynamics by virtue of underactuation. These constraints shape trajectories for the 3D ballbot system, which is a 3D are second-order nonholonomic [8] constraints, i.e., non- omnidirectional wheeled inverted pendulum, are also presented. integrable acceleration/dynamic constraints. These constraints restrict the family of trajectories that the configurations can I. INTRODUCTION follow. In control of balancing underactuated systems, which Underactuated mechanical systems are systems with fewer are destabilized by gravitational forces, it is important to independent control inputs than the degrees of freedom [1]. maintain balance. Maintaining balance makes it difficult to Examples of underactuated systems can be found in robotic, accurately track any desired configuration trajectories. aerospace, marine and locomotive applications. These systems In this paper, we propose a planning procedure to ensure are underactuated due to reasons like higher-order dynam- good approximate tracking of desired trajectories for such ics (e.g., spacecraft, helicopter, underwater vehicles, flexible underactuated balancing systems. For example, consider a robots), actuator failures (e.g., a failed manipulator joint, 3D omnidirectional wheeled inverted pendulum robot like the aircraft engine failure), and need for dynamic locomotion ballbot [9]. We would like it to track some path on the floor, platforms (e.g., Segway [2], ballbots [3]). A set of intention- which could be a desired path from some global motion ally designed underactuated systems used in controls research planning algorithm. The question to be answered here is: how include Acrobot, Pendubot, Cart-Pole, Beam-Ball and Rotating can the wheel/ball be moved along this path while balancing Pendulum [4]. the pendulum? The answer to this question is not trivial In robotics, balancing (dynamically stable) mobile robots because of the dynamic coupling between the motion of the form a special class of underactuated systems. They include ball and the pendulum. Now, consider a balancing controller, wheeled robots like Segway [2], ballbots [3] and legged robots as in [9], which ensures accurate tracking of desired lean like BigDog [5], MABEL [6]. Balancing robots will play a trajectories for the pendulum, then the question, stated above, vital role in realizing the dream of placing robot workers in can be reformulated as: how can the lean trajectories of the human environments. Unlike statically stable mobile robots, pendulum be chosen so that the ball moves along the desired balancing (dynamically stable) mobile robots can: (i) be tall path? This is the question that is addressed in this paper. and skinny with high centers of gravity, (ii) have smaller In order to answer the above question, we should look footprints, and (iii) accelerate or decelerate quickly [3]. They into the nonlinear dynamics of the system. Given an accurate can also be very effective mobile manipulators [7] with model, there have been a variety of controllers designed the ability to maintain postural stability, generate forces on in nonlinear control literature, which use partial feedback external objects and withstand greater impact forces. These linearization [10], to ensure approximate output tracking [11], characteristics make them ideal for navigation and operation [12]. But in real robots, we have to deal with model un- in cluttered human environments. Two-wheeled balancing certainties, higher-order dynamics, nonlinear friction effects and disturbances. Controllers that cancel out nonlinear terms, variables that do not appear in the inertia matrix are called assuming their accurate knowledge, can fail miserably on external variables (qx), i.e., ∂M(q)/∂qx = 0. real robots. Though the model in-hand may not be accurate, Since the inertia matrix is independent of the external 1 T it can often provide some useful information such as the variables, the kinetic energy K(q, q˙) = 2 q˙ M(q)˙q is also dynamic constraints. In [13], an offline trajectory planning independent of the external variables, i.e., ∂K(q, q˙)/∂qx = 0. procedure that produces a class of parametric trajectories In this case, the Lagrangian system is said to have kinetic for the unactuated shape variables to reach static desired symmetry [4]. configurations using the dynamic constraint was presented. Eq. 2 can be re-written as: This planning procedure combined with 2-DOF control, an inner balancing control loop and an outer tracking control loop Mxx(qs) Mxs(qs) q¨x hx(q, q˙) Fx(q) [9], proved to be very successful on the real robot. But, there + = τ, (3) Msx(qs) Mss(qs) q¨s hs(q, q˙) Fs(q) are a few drawbacks with this approach: (i) it was not easily T generalizable to arbitrary motions; (ii) it was computationally where, h(q, q˙)=[hx(q, q˙),hs(q, q˙)] is: expensive; and (iii) it was an offline procedure. In this paper, we present a generalized approach to generate hx(q, q˙) Cxx(q, q˙) Cxs(q, q˙) q˙x Gx(q) shape trajectories (e.g., lean angle trajectories in the ballbot), = + . (4) hs(q, q˙) Csx(q, q˙) Css(q, q˙) q˙s Gs(q) exploiting the structure of dynamic constraints for shape- accelerated underactuated balancing systems, which when The underactuated systems can be classified based on tracked will result in optimal tracking of the desired motion. whether the shape variables qs are fully actuated, partially The advantages of this planning procedure are: (i) it can track actuated or unactuated and based on the presence or lack of any desired trajectories satisfying certain conditions (discussed input couplings in the force matrix F (q) [4]. in Sec. III-B); ( ) it is computationally less expensive; and ii B. Shape-Accelerated Underactuated Balancing Systems (iii) it is fast and can be performed online on the real robot. This paper is organized as follows: Sec. II presents the In this paper, we are interested in shape-accelerated un- properties of shape-accelerated underactuated balancing sys- deractuated balancing systems, which form a special class of tems; Sec. III discusses the special structure of the dynamic underactuated systems satisfying the following properties: constraint for such systems and explains the shape trajectory (i) The shape variables are unactuated and there is no input planning procedure that ensures optimal tracking of desired coupling, e.g., Fx(q)= Im and Fs(q) = 0. A variety of external configuration trajectories; Sec. IV presents the results underactuated systems can be transformed into no input of the optimal shape trajectory planner for a 3D ballbot model; coupling form with global change of coordinates [4]. and finally, Sec. VI presents the conclusion. (ii) There are equal number of actuated and unactuated variables, i.e., dim(qx) = dim(qs) = m and hence II. UNDERACTUATED MECHANICAL SYSTEMS n = 2m. The forced Euler-Lagrange equations of motion for a me- In [4], underactuated systems satisfying properties (i)-(ii) chanical system are: are referred to as Class − IIa underactuated systems. (iii) The potential energy is independent of , , d ∂L ∂L V (q) qx i.e. − = F (q)τ, (1) ∂V (q)/∂q = 0. This implies that G(q)= ∂V (q)/∂q is dt ∂q˙ ∂q x also independent of qx. Since both kinetic and potential Rn where, q ∈ is the configuration vector, L (q, q˙) = energies are independent of qx, the Lagrangian L is also K(q, q˙) − V (q) is the Lagrangian with kinetic energy K independent of qx, i.e., L is symmetric w.r.t. qx. Rm and potential energy V , τ ∈ is the control input and (iv) Mxx(qs) is constant, i.e., ∂Mxx(qs)/∂qs = 0. Rn×m F (q) ∈ is the force matrix. (v) Msx(qs) has differentially symmetric rows, i.e., i iT i A mechanical system satisfying Eq. 1 is said to be an ∂Msx(qs)/∂qs = ∂M sx(qs)/∂qs, where Msx(qs) th underactuated system [1] if m < n, i.e., there are fewer refers to the i row of Msx(qs). independent control inputs than configuration variables. Properties (iii)-(v) make h(q, q˙) independent of both qx Eq. 1 for an underactuated system can be written in matrix and q˙x. form as follows: (vi) The system has locally strong inertial coupling [1], i.e., rank(M (q )) = n − m = m for all q in the neigh- M(q)¨q + C(q, q˙)˙q + G(q)= F (q)τ, (2) sx s borhood of the origin, where origin is the unstable equi- where, M(q) ∈ Rn×n is the inertia matrix, C(q, q˙) ∈ Rn×n is librium. A system satisfying the (local) strong inertial the matrix of Coriolis and centrifugal terms and G(q) ∈ Rn×1 coupling is also known as Internal/External Convertible is the vector of gravitational forces. System [12]. (vii) ∂Gs(qs)/∂qs 6= 0 at qs = 0 and is invertible. A. External and Shape Variables −1 (viii) ∂(Msx(qs) Gs(qs))/∂qs 6= 0 at qs = 0 and is invert- The configuration variables that appear in the inertia matrix ible. are called shape variables (qs), whereas, the configuration The significance of properties (vi)-(viii) will be explained in Sec. III, where they are used for the optimal shape into first-order nonholonomic constraints. This is ensured by trajectory planner design. the fact that the gravitational vector G(qs) is not a constant and −1 (ix) rank(Mss(qs)) = m,i.e.,Mss(qs) exists. the inertia matrix M(qs) is dependent on the unactuated shape −1 (x) ∂(Mss(qs) Gs(qs))/∂qs 6= 0 at qs = 0 and is invert- variables qs. For a detailed discussion of these conditions, refer ible. to [14]. −1 (xi) Mss(qs) Msx(qs) 6= 0 at qs = 0. Due to the properties listed in Sec. II-B, the dynamic Properties (ix)-(xi) ensure that the Jacobian linearization constraint equations for shape-accelerated underactuated bal- (A, B) of the system (Eq. 3) at origin is controllable ancing systems are independent of the position and velocity and its zero dynamics [10] is unstable at the origin. of external variables. This special structure relates the ac- According to [12], an underactuated system that satisfies celeration of external variables to the position, velocity and properties (ix)-(xi) is called a balance system and in this acceleration of shape variables. In this section, we will attempt paper, we will refer to it as a balancing system. at understanding this relationship. The shape-accelerated underactuated balancing systems will Let’s consider the function have equations of motion of the form: ′ Φ (qs, q¨x) = Φ(qs, 0, 0, q¨x) M M (q ) q¨ h (q , q˙ ) τ xx xs s x + x s s = , (5) = Msx(qs)¨qx + hs(qs, 0) Msx(qs) Mss(qs) q¨s hs(qs, q˙s) 0 = Msx(qs)¨qx + Gs(qs). (9) where, It follows from Eq. 8 that hx(qs, q˙s) 0 Cxs(qs, q˙s) q˙x 0 ′ = + . (6) Φ (qs, q¨x) = 0. (10) hs(qs, q˙s) 0 Css(qs, q˙s) q˙s Gs(qs) ′ ′ We can see from Eq. 5 that the equations of motion of these By implicit function theorem, if both ∂Φ /∂qs and ∂Φ /∂q¨x systems are such that any non-zero shape configuration will at (qs, q¨x) = (0, 0) exist and are invertible, then there exists result in acceleration of the external variables and in turn an invertible map Γ : qs → q¨x in the neighborhood of the ′ ′ −1 acceleration of the entire system, hence the name shape- origin such that Φ (qs, Γ(qs)) = 0 and Φ (Γ (¨qx), q¨x) = 0. accelerated underactuated balancing systems. From Eq. 9, we get Some examples of shape-accelerated underactuated bal- ′ ∂Φ ∂G (q ) ancing systems are planar and 3D cart-pole system with = s s , (11) ∂q ∂q unactuated lean angles, planar wheeled inverted pendulum s (qs,q¨x)=(0,0) s qs=0

(e.g. Segway [2] in a plane) and 3D omnidirectional wheeled ∂Φ′ = M (q ) . (12) inverted pendulum (e.g. the ballbot [13, 9]). ∂q¨ sx s x (qs,q¨x)=(0,0) qs=0

III. DYNAMIC CONSTRAINT-BASED OPTIMAL SHAPE From properties (vi) and (vii) in Sec. II-B, we can see that TRAJECTORY PLANNER the Jacobians in both Eq. 11 and Eq. 12 exist and are invertible In this paper, our objective is to plan shape trajectories, and hence, there exists an invertible map Γ : qs → q¨x in the ′ which when tracked will result in optimal tracking of desired neighborhood of the origin such that Φ (qs, Γ(qs)) = 0 and ′ −1 external configuration trajectories. The planning procedure Φ (Γ (¨qx), q¨x) = 0. Γ can be derived directly from Eq. 9 presented in this section exploits the special structure of and Eq. 10 as follows: dynamic constraints of shape-accelerated underactuated balancing systems described in Sec. II-B. So, let’s first look at − q¨ = −M (q ) 1G (q ) the dynamic constraint, its structure and the information that x sx s s s it provides. = Γ(qs) (13) A. Dynamic Constraint in the neighborhood of the origin. Jacobian linearization of Eq. 13 w.r.t. q at q = 0 gives The second set of m equations of motion associated with s s the unactuated shape variables in Eq. 5 given by ∂q¨ ∂(M (q )−1G (q )) x = − sx s s s Msx(qs)¨qx + Mss(qs)¨qs + hs(qs, q˙s) = 0 (7) ∂q ∂q s qs=0 s qs=0 0 can be written as: = Kqs . (14) 0 Φ(qs, q˙s, q¨s, q¨x) = 0. (8) We know that Kqs is invertible by property (viii) in Sec. II-B. This implies, we have a linear map K0 =(K0 )−1 such that Eq. 7 and Eq. 8 are called second-order nonholonomic con- qx qs straints, nonholonomic acceleration constraints, or dynamic q = K0 q¨ , (15) constraints because there exists no function Ψ such that s qx x Ψ¨ = Φ(qs, q˙s, q¨s, q¨x). The dynamic constraint equations are and ′ 0 not even partially integrable, i.e., they cannot be converted Φ (Kqx q¨x, q¨x) = 0 (16) p ′ d ...d ....d in the neighborhood of the origin. where, q¨x(t)=Γ (Kqx q¨x(t),Kqx q x(t),Kqx q x(t)). It is to 2 We can see that q¨x is a constant if and only if qs is a be noted that the parameter space is m -dimensional and any constant. In order for the external configuration of the system optimization algorithm that solves a nonlinear least-squares d in Eq. 5 to have a constant desired acceleration q¨x, the system problem can be used. d 0 should stick to a constant shape configuration given by qs = For a desired constant acceleration trajectory, Kqx = Kqx 0 d d 0 Kqx q¨x. ensures optimality. For any general q¨x(t), Kqx = Kqx may When q¨x is not constant, q˙s and q¨s are non-zero. Let’s take not necessarily ensure optimality but will act as a good initial a = (qs, q˙s, q¨s) and b =q ¨x and then Eq. 8 can be written as guess for the optimization process. Φ(a,b) = 0. Taking the Jacobian w.r.t. b at (a,b)=(0, 0), we As one can see, the optimal shape trajectory planner de- p d get scribed above ensures that q¨x(t) approximately tracks q¨x(t) p d but does not ensure that qx(t) approximately tracks qx(t) or ∂Φ(a,b) p d = M (q ) (17) q˙x(t) approximately tracks q˙x(t). This can be ensured only if ∂b sx s (a,b)=(0,0) qs=0 the initial conditions for the external variables are met, i.e., p d p d From property (vi) in Sec. II-B, we can see that Eq. 17 exists qx(0) = qx(0) and q˙x(0)=q ˙x(0). Given below are the conditions to be met in order to use and is invertible. Hence, by implicit function theorem, there d ′ ′ the shape trajectory planner described above to track : exists a map Γ : a → b such that Φ(a, Γ (a)) = 0. The map qx(t) ′ d 2 d d is not invertible since at exists (i) qx(t) must at least be of class C , i.e., q˙x(t) and q¨x(t) Γ ∂Φ(a,b)/∂a (a,b)=(0, 0) d but is not invertible. exist and are continuous. If q¨x(t) does not exist then, p We can see that in order to track a non-constant, time- the planned shaped trajectory qs (t) that is proportional d varying q¨d(t), there is no function that maps q¨d(t) to to q¨x(t) will not exist as well. x x d 4 d d d (ii) qx(t) is preferred to be of class C , i.e., first four (qs (t), q˙s (t), q¨s (t)) such that the dynamic constraints in Eq. 8 are satisfied. In the following subsection, we propose an derivatives exist and are continuous so as to ensure the p p p optimal planner that ensures approximate tracking of q¨d(t). existence of qs (t), q˙s (t), q¨s (t) that depend on them. This x condition also avoids discontinuities in the planned shape B. Optimal Shape Trajectory Planner trajectories and its first two derivatives. In underactuated balancing systems, we are often interested (iii) Initial conditions for the external variables are met, i.e., p d p d in tracking desired trajectories for the external configuration qx(0) = qx(0) and q˙x(0) =q ˙x(0). The desired position d variables without losing balance. Shape-accelerated underac- trajectory qx(t) can be tracked by approximate tracking d tuated balancing systems in Sec. II-B have constraints on of q¨x(t) only if the system starts at the correct initial the acceleration of these external variables w.r.t. the shape position and velocity for the external variables. variables’ position, velocity and acceleration as given below: IV. EXAMPLE:3DBALLBOT MODEL −1 q¨x = −Msx(qs) (Mss(qs)¨qs + hs(qs, q˙s)) As an example, we present here the results of the optimal ′ shape trajectory planner described in Sec. III-B for a 3D = Γ (qs, q˙s, q¨s) (18) ballbot model, which is a 3D omnidirectional wheeled inverted So, for a desired acceleration trajectory for the external pendulum. The ballbot (Fig. 1(a)) is modeled as a rigid d configuration q¨x(t), we would like to plan shape configura- cylindrical body on top of a rigid spherical wheel/ball with p p p tion trajectories (qs (t), q˙s (t), q¨s (t)), which when tracked will the following assumptions: (i) there is no slip between the p p d result in q¨x(t) (from Eq. 18) such that q¨x(t) =q ¨x(t). But, ball and the floor, and (ii) there is no yaw/spinning motion ′−1 we have seen that Γ :q ¨x(t) → (qs(t), q˙s(t), q¨s(t)) that for both the body and the ball, i.e., they have two degrees-of- ′−1 satisfies Φ(Γ (¨qx(t)), q¨x(t)) = 0 does not exist. Hence, freedom each. It is to be noted that the results presented here d p p p we propose to find a map Ω :q ¨x(t) → (qs (t), q˙s (t), q¨s (t)) are that of simulation and are not experimental results. p d 2 p such that kq¨x(t) − q¨x(t)k2 is minimized. Here, q¨x(t) = The forced Euler-Lagrange equations of motion are given ′ p p p Γ (qs (t), q˙s (t), q¨s (t)). by Eq. 1 with qx corresponding to the configuration of the

Inspired from Eq. 15, we propose to use a linear map Kqx : ball/wheel, and qs corresponding to the configuration of the d p T T q¨x → qs such that body (Euler angles). Here, qx =[θx,θy] and qs =[φx,φy] . Planar versions of the ball and body configurations are shown p d in Fig. 1(b). qs (t)= Kqx q¨x(t), (19) The ball configurations, θx and θy, are chosen such that p ...d q˙s (t)= Kqx q x(t), (20) the linear position of the ball/wheel (with radius r) on the .... p d XY ground plane is given by x = rθ and y = rθ . This q¨s (t)= Kqx q x(t). (21) w x w y produces input coupling in F (q): The shape trajectory planning procedure can now be formu- 1 0 lated as an optimization problem, where the elements of Kqx  0 1  are determined with the objective of minimizing the function F (q)= . (23) 0 −1 p d   J = kq¨ (t) − q¨ (t)k2, (22)  −1 0  x x 2   1) Straight Line Motion: Let’s start with the simplest of trajectories that involve moving along a straight line between static configurations, i.e., starting from rest at one point on the floor and coming to rest at another point on the floor. The th desired xw(t) and yw(t) are chosen to be nonic (9 degree) polynomials in t so that their first four derivatives satisfy the boundary conditions.

mbody

2 Desired Planned

()a ()b 1 Fig. 1. (a) The ballbot balancing, (b) Planar ballbot model with ball and body conﬁgurations shown. Linear Y Position (m)

′ ′ By global change of coordinates θx = θx − φy and θy = θy + φx, the equations of motion with the new configuration vector q′ =[θ′ ,θ′ ,φ ,φ ]T has no input coupling in F ′(q′): x y x y 0 0 1 2 1 0 Linear X Position (m) ′ ′  0 1  F (q )= . (24) 0 0   Fig. 2. Straight Line Motion - Linear XY (This figure is best viewed in  0 0    color.) The new forced Euler-Lagrange equations are: ′ ′ ′ ′ ′ ′ ′ ′ ′ ′ ′ M (q )¨q + C (q , q˙ )˙q + G (q )= F (q )τ. (25) 9 i It is to be noted that the underactuated system in Eq. 25 xw(t)= ait , satisfies all the properties of shape-accelerated underactuated Xi=0 systems and balancing systems listed in Sec. II-B. The expres- 9 ′ ′ ′ i sions for M ,C , G are omitted here due to lack of space. yw(t)= bit , (26) The last two equations of motion in Eq. 25 form the Xi=0 dynamic constraint equations of the system and these equa- where the coefficients ais and bis are determined based on tions are used for the optimal shape trajectory planner in the initial and final desired configurations. Sec. III-B. The optimization algorithm used here is Levenberg-

Marquardt algorithm (LMA), which is a widely used tool for p minimization problems in least-squares curve fitting and non- 2 φx p linear programming. The dynamic equations were simulated φy in MATLAB and the optimization was implemented using 1 MATLAB’s lsqnonlin function. Some of the planning results are presented below. 0 A. Results of Optimal Shape Trajectory Planning Angle (deg) This section presents a variety of desired xw(t) and yw(t) −1 that satisfy the conditions in Sec. III-B and the corresponding p p planned optimal shape trajectories, φx(t) and φy(t), which should be tracked to achieve them. It is important to note that −2 the desired trajectories, xw(t) and yw(t), are trajectories of the 0 5 10 center of the ball and not trajectories of the system’s center of Time (s) gravity. On a flat floor, these trajectories match the trajectories of the ball’s contact point with the floor. Fig. 3. Straight Line Motion - Planned Shape Trajectories In Fig. 2, we can see the planned XY motion approximately to be noted that the results presented here are observed only tracking the desired XY motion. The planned shape trajectories when the appropriate initial conditions are met. The tracking that produce such a motion are shown in Fig. 3. The trajecto- error statistics are: RMSE = 4.3x10−4 m and Maximum Error ries for the planned XY motion are obtained from Eq. 18. The = 5.6x10−4 m. tracking error statistics are: RMSE = 0.0147 m and Maximum Error = 0.0238 m. 6 Desired 5 4 Planned Desired Planned 4 2

3 0

2 −2 Linear Y Position (m)

1 −4 Linear Y Position (m)

0 −6 −6 −4 −2 0 2 4 6 −1 Linear X Position (m) −3 −2 −1 0 1 2 3 Linear X Position (m) Fig. 6. Figure-8 Motion - Linear XY (This ﬁgure is best viewed in color.)

Fig. 4. Circular Motion - Linear XY (This figure is best viewed in color.) 3) Figure-8 Path: Here, we would like the robot to move along a figure-8 path on the floor. The desired xw(t) and yw(t) 2) Circular Motion: Here, we would like the robot to move are: along a circular path on the floor. The desired x (t) and y (t) w w x (t)= A sin(ω t), are: w x x yw(t)= Ay sin(ωyt), (28) xw(t)= R sin(ωt), where, A = 4 m, ω = π/20 rad/s, A = 4 m, and ω = y (t)= R(1 − cos(ωt)), (27) x x y y w π/10 rad/s. where, R = 2 m and ω = π/10 rad/s. p 2 φx p 2 φx p φy p φy 1 1

0 0 Angle (deg)

Angle (deg) −1 −1

−2 −2 0 10 20 30 40 0 5 10 15 20 Time (s) Time (s) Fig. 7. Figure-8 Motion - Planned Shape Trajectories Fig. 5. Circular Motion - Planned Shape Trajectories In Fig. 6, we can see the planned XY motion approximately In Fig. 4, we can see the planned XY motion approximately tracking the desired XY motion. The planned shape trajectories tracking the desired XY motion. The planned shape trajectories that produce such a motion are shown in Fig. 7. It is to be that produce such a motion are shown in Fig. 5. The trajecto- noted that the results presented here are observed only when ries for the planned XY motion are obtained from Eq. 18. It is the appropriate initial conditions are met. The trajectories for the planned XY motion are obtained from Eq. 18. The tracking In Fig. 8, we can see the planned XY motion approximately error statistics are: RMSE = 0.0105 m and Maximum Error = tracking the desired XY motion. The planned shape trajectories 0.0177 m. that produce such a motion are shown in Fig. 9. The trajectories for the planned XY motion are obtained from Eq. 18. The tracking error statistics are: RMSE = 0.0167 m and Maximum 6 Error = 0.0286 m. Desired 5 Planned B. Real-Time Planning For all the results presented in Sec. IV-A, the optimization 4 tolerance values for both the residual norm and parameter values were set to < 10−4. On a standard Intel Core-2 Duo processor, the optimization implementation in MATLAB 3 converges in < 1 second. This ensures that the optimal shape trajectory planner presented in this paper can be used for real- 2 time planning on the robot. Linear Y Position (m)

1 V. BALANCING AND TRACKING CONTROL The entire planning procedure presented in this paper as- 0 sumes that there exists a balancing controller, similar to the −3 −2 −1 0 1 2 3 one in [9], which has good shape trajectory tracking perfor- Linear X Position (m) mance. Given the balancing controller and the optimal shape trajectory planner, we can achieve good approximate tracking Fig. 8. Join-Circle-Leave Motion - Linear XY (This figure is best viewed of the desired external configuration trajectories. But it is to in color.) be noted that this tracking is open-loop and with wrong initial conditions, we have no way to ensure approximate tracking of 4) Join-Circle-Leave Motion: The tracking of a circle, sine the desired external configuration trajectories. Moreover, while wave and figure-8 paths require that the appropriate initial testing on the real robot, modeling uncertainties, unmodeled conditions are met. But in reality, we would like to start at dynamics, nonlinear friction effects and noise may prevent any random initial configuration and track a particular path good approximate tracking. Hence, we should have an external on the floor. Here, we present the result of starting from trajectory tracking controller, similar to the one in [13], to rest, joining a circular path at a desired configuration, making ensure better tracking. 1.5 revolutions, leaving the circular path at a diametrically opposite point and coming to rest. This motion consists of 3 d Shape-Accelerated qs t qx different trajectories fused together. The joining and leaving Balancing Underactuated q trajectories are nonic (9th degree) polynomials that satisfy the + Controller BalancingSystems s - initial and final configurations required to join and leave the circular path respectively. The nonic polynomials ensure that the first four derivatives of the fused trajectories are continuous q c - and not just piecewise continuous. s Tracking + d qx + Controller + p 2 φx p qs OptimalShape p TrajectoryPlanner φy 1 Fig. 10. Control Architecture

0 We propose to follow [13] in using the control architecture shown in Fig. 10 for good approximate tracking of desired

Angle (deg) d external configuration trajectories (qx) on the real robot. The −1 d balancing controller tracks the desired shape trajectories (qs ), p c which are a sum of planned (qs ) and compensation (qs) shape −2 trajectories. The planned shape trajectories are given by the 0 10 20 30 40 50 60 optimal shape trajectory planner, whereas, the compensation Time (s) shape trajectories are provided by the tracking controller, which tries to compensate for the deviation of the external Fig. 9. Join-Circle-Leave Motion - Planned Shape Trajectories configuration trajectories from the desired ones. In Fig. 11, we show the result of the control architecture with the optimal shape planner can ensure better tracking when in Fig. 10 tracking the external configuration trajectories the initial conditions are not met. corresponding to a circular motion starting with zero initial In the 3D ballbot example, we have seen that given d d 4 conditions, i.e., there is no initial linear velocity in the x (xw(t),yw(t)) ∈ C , we can use the optimal shape trajectory direction, as in Fig. 4. We can see that the motion asymp- planner to obtain the desired shape trajectories, which when totically converges to the circular motion, which shows the tracked will result in the desired motion. In a general motion effectiveness of the tracking controller in compensating for planning setting, any standard path planning algorithm like A∗ the errors due to wrong initial conditions. can be used to obtain a desired (xw(s),yw(s)) path. In order to use the optimal shape trajectory planner discussed in this paper as a tool for overall motion planning, we have to parametrize 5 this path in time. We propose that using nonic splines, which Desired ensure that the first four derivatives for the overall trajectory will be continuous throughout its domain, will help us achieve 4 Actual this goal. Future work will include experimental testing of the planned shape trajectories presented here and design of a 3 global motion planner that provides desired external configuration trajectories for the optimal shape trajectory planner. 2 VII. ACKNOWLEDGEMENTS This work was supported in part by NSF grants IIS-0308067 1 and IIS-0535183. The author thanks Ralph Hollis, George Linear Y Position (m) Kantor, Howie Choset and RSS reviewers for their valuable 0 comments that helped in improving this paper.

REFERENCES −1 −3 −2 −1 0 1 2 3 [1] M. W. Spong, “The control of underactuated mechanical systems,” in First International Conference on Mechatronics, Mexico City, 1994. Linear X Position (m) [2] H. G. Nguyen, J. Morrell, K. Mullens, A. Burmeister, S. Miles, N. Far- rington, K. Thomas, and D. Gage, “Segway robotic mobility platform,” Fig. 11. Tracking Circular Motion with Zero Initial Conditions (This figure in SPIE Proc. 5609: Mobile Robots XVII, Philadelphia, PA, October is best viewed in color.) 2004. [3] R. Hollis, “Ballbots,” Scientific American, pp. 72–78, October 2006. [4] R. Olfati-Saber, “Nonlinear control of underactuated mechanical systems The contribution of this paper is the optimal shape trajectory with application to robotics and aerospace vehicles,” Ph.D. dissertation, planner, whereas, the balancing and tracking controller designs Massachusetts Institute of Technology, February 2001. [5] M. Raibert, K. Blankespoor, G. Nelson, R. Playter, and the Big- are borrowed from our previous work [13]. In [13], we have Dog Team, “Bigdog, the rough-terrain quadraped robot,” in In Proc. 17th shown that the control architecture in Fig. 10 is capable of World Congress of the International Federation of Automatic Control, successfully tracking straight line motions on the real robot. 2008, pp. 10822–10825. [6] J. Grizzle, J. Hurst, B. Morris, H.-W. Park, and K. Sreenath, “Mabel, a Based on these previous successful experimental results, we new robotic bipedal walker and runner,” in American Control Confer- strongly believe that this control architecture will enable the ence, St. Louis, MO, June 2009. real robot to track the motions presented in Sec. IV-A. It [7] P. Deegan, B. Thibodeau, and R. Grupen, “Designing a self-stabilizing robot for dynamic mobile manipulation,” in Robotics: Science and is to be noted that the planner in [13] was designed based Systems - Workshop on Manipulation for Human Environments, August on our intuition of how the robot should lean in order to 2006. achieve a straight line motion, whereas, in this paper, we [8] J. R. Ray, “Nonholonomic constraints,” American Journal of Physics, vol. 34, pp. 406–408, 1966. have presented a generalized shape trajectory planner that will [9] U. Nagarajan, A. Mampetta, G. Kantor, and R. Hollis, “State transition, enable the ballbot to approximately track any desired external balancing, station keeping, and yaw control for a dynamically stable configuration trajectory. single spherical wheel mobile robot,” Proc. IEEE Int’l. Conf. on Robotics and Automation, May 2009. [10] A. Isidori, Nonlinear Control Systems. Berlin: Springer-Verlag, 1989. VI. CONCLUSION [11] A. Isidori and C. I. Byrnes, “Output regulation of nonlinear systems,” IEEE Transactions on Automatic Control, vol. 35, no. 2, pp. 131–140, An algorithm to plan optimal shape trajectories for shape- 1990. accelerated underactuated balancing systems was presented. [12] N. H. Getz, “Dynamic inversion of nonlinear maps with applications to nonlinear control and robotics,” Ph.D. dissertation, University of These shape trajectories, which when tracked will result in California at Berkeley, December 1995. approximate tracking of the desired external configuration [13] U. Nagarajan, G. Kantor, and R. Hollis, “Trajectory planning and control trajectories subject to conditions on differentiability of the of an underactuated dynamically stable single spherical wheeled mobile robot,” Proc. IEEE Int’l. Conf. on Robotics and Automation, May 2009. desired trajectory and correct initial conditions. The planning [14] G. Oriolo and Y. Nakamura, “Control of mechanical systems with procedure presented assumes that there exists a balancing second-order nonholonomic constraints: Underactuated manipulators,” controller that can accurately track desired shape trajectories. in In Proc. 30th IEEE Conference on Decision and Control, vol. 3, A feedback trajectory tracking controller used in combination 1991, pp. 2398–2403.