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

Transcription

1 Dynamic Constraint-base Optimal Shape Trajectory Planner for Shape-Accelerate Uneractuate Balancing Systems Umashankar Nagarajan The Robotics Institute Carnegie Mellon University Pittsburgh, PA 53 Abstract This paper presents an optimal shape trajectory planner for shape-accelerate uneractuate balancing systems, which are estabilize by gravitational forces. These systems have unactuate shape variables an fully actuate external variables. They also have the same number of actuate an unactuate egrees of freeom. Their equations of motion result in nonholonomic acceleration/ynamic constraints, which relate the acceleration of external variables to the position, velocity an acceleration of shape variables. This paper escribes a proceure to use the ynamic constraints for planning shape trajectories, which when tracke will result in optimal tracking of esire external configuration trajectories. Examples of planne optimal shape trajectories for the 3D ballbot system, which is a 3D omniirectional wheele inverte penulum, are also presente. I. INTRODUCTION Uneractuate mechanical systems are systems with fewer inepenent control inputs than the egrees of freeom. Examples of uneractuate systems can be foun in robotic, aerospace, marine an locomotive applications. These systems are uneractuate ue to reasons like higher-orer ynamics (e.g., spacecraft, helicopter, unerwater vehicles, flexible robots), actuator failures (e.g., a faile manipulator joint, aircraft engine failure), an nee for ynamic locomotion platforms (e.g., Segway, ballbots 3). A set of intentionally esigne uneractuate systems use in controls research inclue Acrobot, Penubot, Cart-Pole, Beam-Ball an Rotating Penulum 4. In robotics, balancing (ynamically stable) mobile robots form a special class of uneractuate systems. They inclue wheele robots like Segway, ballbots 3 an legge robots like BigDog 5, MABEL 6. Balancing robots will play a vital role in realizing the ream of placing robot workers in human environments. Unlike statically stable mobile robots, balancing (ynamically stable) mobile robots can: (i) be tall an skinny with high centers of gravity, (ii) have smaller footprints, an (iii) accelerate or ecelerate quickly 3. They can also be very effective mobile manipulators 7 with the ability to maintain postural stability, generate forces on external objects an withstan greater impact forces. These characteristics make them ieal for navigation an operation in cluttere human environments. Two-wheele balancing robots, like Segway, have kinematic constraints restricting their irection of motion, whereas, single spherical-wheele balancing robots, like ballbots, have omniirectional motion that make them more suitable for operation in constraine spaces. In this paper, such omniirectional balancing systems are referre to as shape-accelerate uneractuate balancing systems (escribe in Sec. II-B). An interesting an troubling factor in control an planning of such uneractuate systems is the constraint on their ynamics by virtue of uneractuation. These constraints are secon-orer nonholonomic 8 constraints, i.e., nonintegrable acceleration/ynamic constraints. These constraints restrict the family of trajectories that the configurations can follow. In control of balancing uneractuate systems, which are estabilize by gravitational forces, it is important to maintain balance. Maintaining balance makes it ifficult to accurately track any esire configuration trajectories. In this paper, we propose a planning proceure to ensure goo approximate tracking of esire trajectories for such uneractuate balancing systems. For example, consier a 3D omniirectional wheele inverte penulum robot like the ballbot 9. We woul like it to track some path on the floor, which coul be a esire path from some global motion planning algorithm. The question to be answere here is: how can the wheel/ball be move along this path while balancing the penulum? The answer to this question is not trivial because of the ynamic coupling between the motion of the ball an the penulum. Now, consier a balancing controller, as in 9, which ensures accurate tracking of esire lean trajectories for the penulum, then the question, state above, can be reformulate as: how can the lean trajectories of the penulum be chosen so that the ball moves along the esire path? This is the question that is aresse in this paper. In orer to answer the above question, we shoul look into the nonlinear ynamics of the system. Given an accurate moel, there have been a variety of controllers esigne in nonlinear control literature, which use partial feeback linearization, to ensure approximate output tracking,. But in real robots, we have to eal with moel uncertainties, higher-orer ynamics, nonlinear friction effects

2 an isturbances. Controllers that cancel out nonlinear terms, assuming their accurate knowlege, can fail miserably on real robots. Though the moel in-han may not be accurate, it can often provie some useful information such as the ynamic constraints. In 3, an offline trajectory planning proceure that prouces a class of parametric trajectories for the unactuate shape variables to reach static esire configurations using the ynamic constraint was presente. This planning proceure combine with -DOF control, an inner balancing control loop an an outer tracking control loop 9, prove to be very successful on the real robot. But, there are a few rawbacks with this approach: (i) it was not easily generalizable to arbitrary motions; (ii) it was computationally expensive; an (iii) it was an offline proceure. In this paper, we present a generalize approach to generate shape trajectories (e.g., lean angle trajectories in the ballbot), exploiting the structure of ynamic constraints for shapeaccelerate uneractuate balancing systems, which when tracke will result in optimal tracking of the esire motion. The avantages of this planning proceure are: (i) it can track any esire trajectories satisfying certain conitions (iscusse in Sec. III-B); (ii) it is computationally less expensive; an (iii) it is fast an can be performe online on the real robot. This paper is organize as follows: Sec. II presents the properties of shape-accelerate uneractuate balancing systems; Sec. III iscusses the special structure of the ynamic constraint for such systems an explains the shape trajectory planning proceure that ensures optimal tracking of esire external configuration trajectories; Sec. IV presents the results of the optimal shape trajectory planner for a 3D ballbot moel; an finally, Sec. VI presents the conclusion. II. UNDERACTUATED MECHANICAL SYSTEMS The force Euler-Lagrange equations of motion for a mechanical system are: L t q L q = F(q)τ, () where, q R n is the configuration vector, L(q, q) = K(q, q) V(q) is the Lagrangian with kinetic energy K an potential energy V, τ R m is the control input an F(q) R n m is the force matrix. A mechanical system satisfying Eq. is sai to be an uneractuate system if m < n, i.e., there are fewer inepenent control inputs than configuration variables. Eq. for an uneractuate system can be written in matrix form as follows: M(q) q C(q, q) q G(q) = F(q)τ, () where, M(q) R n n is the inertia matrix, C(q, q) R n n is the matrix of Coriolis an centrifugal terms an G(q) R n is the vector of gravitational forces. A. External an Shape Variables The configuration variables that appear in the inertia matrix are calle shape variables (q s ), whereas, the configuration variables that o not appear in the inertia matrix are calle external variables (q x ), i.e., M(q)/ q x =. Since the inertia matrix is inepenent of the external variables, the kinetic energy K(q, q) = qt M(q) q is also inepenent of the external variables, i.e., K(q, q)/ q x =. In this case, the Lagrangian system is sai to have kinetic symmetry 4. Eq. can be re-written as: Mxx (q s ) M xs (q s ) M sx (q s ) M ss (q s ) qx q s hx (q, q) = h s (q, q) where, h(q, q) = h x (q, q),h s (q, q) T is: hx (q, q) Cxx (q, q) C = xs (q, q) h s (q, q) C sx (q, q) C ss (q, q) qx q s Fx (q) τ, (3) F s (q) Gx (q) G s (q). (4) The uneractuate systems can be classifie base on whether the shape variables q s are fully actuate, partially actuate or unactuate an base on the presence or lack of input couplings in the force matrix F(q) 4. B. Shape-Accelerate Uneractuate Balancing Systems In this paper, we are intereste in shape-accelerate uneractuate balancing systems, which form a special class of uneractuate systems satisfying the following properties: (i) The shape variables are unactuate an there is no input coupling, e.g., F x (q) = I m an F s (q) =. A variety of uneractuate systems can be transforme into no input coupling form with global change of coorinates 4. (ii) There are equal number of actuate an unactuate variables, i.e., im(q x ) = im(q s ) = m an hence n = m. In 4, uneractuate systems satisfying properties (i)-(ii) are referre to as Class IIa uneractuate systems. (iii) The potential energy V(q) is inepenent of q x, i.e., V(q)/ q x =. This implies that G(q) = V(q)/ q is also inepenent of q x. Since both kinetic an potential energies are inepenent of q x, the Lagrangian L is also inepenent of q x, i.e., L is symmetric w.r.t. q x. (iv) M xx (q s ) is constant, i.e., M xx (q s )/ q s =. (v) M sx (q s ) has ifferentially symmetric rows, i.e., M sx (q s )/ q i s = M it sx(q s )/ q s, where M i sx(q s ) refers to the i th row of M sx (q s ). Properties (iii)-(v) make h(q, q) inepenent of both q x an q x. (vi) The system has locally strong inertial coupling, i.e., rank(m sx (q s )) = n m = m for all q in the neighborhoo of the origin, where origin is the unstable equilibrium. A system satisfying the (local) strong inertial coupling is also known as Internal/External Convertible System. (vii) G s (q s )/ q s at q s = an is invertible. (viii) (M sx (q s ) G s (q s ))/ q s at q s = an is invertible. The significance of properties (vi)-(viii) will be explaine

3 in Sec. III, where they are use for the optimal shape trajectory planner esign. (ix) rank(m ss (q s )) = m,i.e.,m ss (q s ) exists. (x) (M ss (q s ) G s (q s ))/ q s at q s = an is invertible. (xi) M ss (q s ) M sx (q s ) at q s =. Properties (ix)-(xi) ensure that the Jacobian linearization (A,B) of the system (Eq. 3) at origin is controllable an its zero ynamics is unstable at the origin. Accoring to, an uneractuate system that satisfies properties (ix)-(xi) is calle a balance system an in this paper, we will refer to it as a balancing system. The shape-accelerate uneractuate balancing systems will have equations of motion of the form: Mxx M xs (q s ) M sx (q s ) M ss (q s ) where, hx (q s, q s ) h s (q s, q s ) = qx q s Cxs (q s, q s ) C ss (q s, q s ) hx (q s, q s ) h s (q s, q s ) qx q s τ =, (5) G s (q s ). (6) We can see from Eq. 5 that the equations of motion of these systems are such that any non-zero shape configuration will result in acceleration of the external variables an in turn acceleration of the entire system, hence the name shapeaccelerate uneractuate balancing systems. Some examples of shape-accelerate uneractuate balancing systems are planar an 3D cart-pole system with unactuate lean angles, planar wheele inverte penulum (e.g. Segway in a plane) an 3D omniirectional wheele inverte penulum (e.g. the ballbot 3, 9). III. DYNAMIC CONSTRAINT-BASED OPTIMAL SHAPE TRAJECTORY PLANNER In this paper, our objective is to plan shape trajectories, which when tracke will result in optimal tracking of esire external configuration trajectories. The planning proceure presente in this section exploits the special structure of ynamic constraints of shape-accelerate uneractuate balancing systems escribe in Sec. II-B. So, let s first look at the ynamic constraint, its structure an the information that it provies. A. Dynamic Constraint The secon set of m equations of motion associate with the unactuate shape variables in Eq. 5 given by can be written as: M sx (q s ) q x M ss (q s ) q s h s (q s, q s ) = (7) Φ(q s, q s, q s, q x ) =. (8) Eq. 7 an Eq. 8 are calle secon-orer nonholonomic constraints, nonholonomic acceleration constraints, or ynamic constraints because there exists no function Ψ such that Ψ = Φ(q s, q s, q s, q x ). The ynamic constraint equations are not even partially integrable, i.e., they cannot be converte into first-orer nonholonomic constraints. This is ensure by the fact that the gravitational vectorg(q s ) is not a constant an the inertia matrix M(q s ) is epenent on the unactuate shape variablesq s. For a etaile iscussion of these conitions, refer to 4. Due to the properties liste in Sec. II-B, the ynamic constraint equations for shape-accelerate uneractuate balancing systems are inepenent of the position an velocity of external variables. This special structure relates the acceleration of external variables to the position, velocity an acceleration of shape variables. In this section, we will attempt at unerstaning this relationship. Let s consier the function Φ (q s, q x ) = Φ(q s,,, q x ) It follows from Eq. 8 that = M sx (q s ) q x h s (q s,) = M sx (q s ) q x G s (q s ). (9) Φ (q s, q x ) =. () By implicit function theorem, if both Φ / q s an Φ / q x at (q s, q x ) = (,) exist an are invertible, then there exists an invertible map Γ : q s q x in the neighborhoo of the origin such that Φ (q s,γ(q s )) = an Φ (Γ ( q x ), q x ) =. From Eq. 9, we get Φ q s = G s(q s ) (qs, q x)=(,) q s () qs=, Φ q x = M sx (q s ) () (qs, q x)=(,) qs=. From properties (vi) an (vii) in Sec. II-B, we can see that the Jacobians in both Eq. an Eq. exist an are invertible an hence, there exists an invertible map Γ : q s q x in the neighborhoo of the origin such that Φ (q s,γ(q s )) = an Φ (Γ ( q x ), q x ) =. Γ can be erive irectly from Eq. 9 an Eq. as follows: q x = M sx (q s ) G s (q s ) = Γ(q s ) (3) in the neighborhoo of the origin. Jacobian linearization of Eq. 3 w.r.t. q s at q s = gives q x q s qs= = (M sx(q s ) G s (q s )) q s qs= = K q s. (4) We know that K q s is invertible by property (viii) in Sec. II-B. This implies, we have a linear map K q x = (K q s ) such that an q s = K q x q x, (5) Φ (K q x q x, q x ) = (6)

4 in the neighborhoo of the origin. We can see that q x is a constant if an only if q s is a constant. In orer for the external configuration of the system in Eq. 5 to have a constant esire acceleration q x, the system shoul stick to a constant shape configuration given by qs = Kq x q x. When q x is not constant, q s an q s are non-zero. Let s take a = (q s, q s, q s ) an b = q x an then Eq. 8 can be written as Φ(a,b) =. Taking the Jacobian w.r.t. b at (a,b) = (,), we get Φ(a, b) b = M sx (q s ) (a,b)=(,) qs= (7) From property (vi) in Sec. II-B, we can see that Eq. 7 exists an is invertible. Hence, by implicit function theorem, there exists a map Γ : a b such that Φ(a,Γ (a)) =. The map Γ is not invertible since Φ(a,b)/ a at (a,b) = (,) exists but is not invertible. We can see that in orer to track a non-constant, timevarying q x(t), there is no function that maps q x(t) to (q s(t), q s(t), q s(t)) such that the ynamic constraints in Eq. 8 are satisfie. In the following subsection, we propose an optimal planner that ensures approximate tracking of q x(t). B. Optimal Shape Trajectory Planner In uneractuate balancing systems, we are often intereste in tracking esire trajectories for the external configuration variables without losing balance. Shape-accelerate uneractuate balancing systems in Sec. II-B have constraints on the acceleration of these external variables w.r.t. the shape variables position, velocity an acceleration as given below: q x = M sx (q s ) (M ss (q s ) q s h s (q s, q s )) = Γ (q s, q s, q s ) (8) So, for a esire acceleration trajectory for the external configuration q x(t), we woul like to plan shape configuration trajectories (q p s(t), q p s(t), q p s(t)), which when tracke will result in q p x(t) (from Eq. 8) such that q p x(t) = q x(t). But, we have seen that Γ : q x (t) (q s (t), q s (t), q s (t)) that satisfies Φ(Γ ( q x (t)), q x (t)) = oes not exist. Hence, we propose to fin a map Ω : q x(t) (q p s(t), q p s(t), q p s(t)) such that q p x(t) q x(t) is minimize. Here, q p x(t) = Γ (q p s(t), q p s(t), q p s(t)). Inspire from Eq. 5, we propose to use a linear map K qx : q x q p s such that q p s(t) = K qx q x(t), (9) q p s(t) = K qx... q x(t), () q p s(t) = K qx... q x(t). () The shape trajectory planning proceure can now be formulate as an optimization problem, where the elements of K qx are etermine with the objective of minimizing the function J = q p x(t) q x(t), () where, q x(t) p = Γ (K qx q x(t),k... qx q... x(t),k qx q x(t)). It is to be note that the parameter space is m -imensional an any optimization algorithm that solves a nonlinear least-squares problem can be use. For a esire constant acceleration trajectory, K qx = Kq x ensures optimality. For any general q x(t), K qx = Kq x may not necessarily ensure optimality but will act as a goo initial guess for the optimization process. As one can see, the optimal shape trajectory planner escribe above ensures that q x(t) p approximately tracks q x(t) but oes not ensure that qx(t) p approximately tracks qx(t) or q x(t) p approximately tracks q x(t). This can be ensure only if the initial conitions for the external variables are met, i.e., qx() p = qx() an q x() p = q x(). Given below are the conitions to be met in orer to use the shape trajectory planner escribe above to track qx(t): (i) qx(t) must at least be of class C, i.e., q x(t) an q x(t) exist an are continuous. If q x(t) oes not exist then, the planne shape trajectory qs(t) p that is proportional to q x(t) will not exist as well. (ii) qx(t) is preferre to be of class C 4, i.e., first four erivatives exist an are continuous so as to ensure the existence of qs(t), p q s(t), p q s(t) p that epen on them. This conition also avois iscontinuities in the planne shape trajectories an its first two erivatives. (iii) Initial conitions for the external variables are met, i.e., qx() p = qx() an q x() p = q x(). The esire position trajectory qx(t) can be tracke by approximate tracking of q x(t) only if the system starts at the correct initial position an velocity for the external variables. IV. EXAMPLE: 3D BALLBOT MODEL As an example, we present here the results of the optimal shape trajectory planner escribe in Sec. III-B for a 3D ballbot moel, which is a 3D omniirectional wheele inverte penulum. The ballbot (Fig. (a)) is moele as a rigi cylinrical boy on top of a rigi spherical wheel/ball with the following assumptions: (i) there is no slip between the ball an the floor, an (ii) there is no yaw/spinning motion for both the boy an the ball, i.e., they have two egrees-offreeom each. It is to be note that the results presente here are that of simulation an are not experimental results. The force Euler-Lagrange equations of motion are given by Eq. with q x corresponing to the configuration of the ball/wheel, an q s corresponing to the configuration of the boy (Euler angles). Here, q x = θ x,θ y T an q s = φ x,φ y T. Planar versions of the ball an boy configurations are shown in Fig. (b). The ball configurations, θ x an θ y, are chosen such that the linear position of the ball/wheel (with raius r) on the XY groun plane is given by x w = rθ x an y w = rθ y. This prouces input coupling in F(q): F(q) =. (3)

5 ) 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 an coming to rest at another point on the floor. The esire x w (t) an y w (t) are chosen to be nonic (9 th egree) polynomials in t so that their first four erivatives satisfy the bounary conitions. m boy Desire Planne ( a) Fig.. (a) The ballbot balancing, (b) Planar ballbot moel with ball an boy configurations shown. r ( b) By global change of coorinates θ x = θ x φ y an θ y = θ y φ x, the equations of motion with the new configuration vector q = θ x,θ y,φ x,φ y T has no input coupling in F (q ): F (q ) = The new force Euler-Lagrange equations are:. (4) M (q ) q C (q, q ) q G (q ) = F (q )τ. (5) It is to be note that the uneractuate system in Eq. 5 satisfies all the properties of shape-accelerate uneractuate systems an balancing systems liste in Sec. II-B. The expressions for M,C,G are omitte here ue to lack of space. The last two equations of motion in Eq. 5 form the ynamic constraint equations of the system an these equations are use for the optimal shape trajectory planner in Sec. III-B. The optimization algorithm use here is Levenberg- Marquart algorithm (LMA), which is a wiely use tool for minimization problems in least-squares curve fitting an nonlinear programming. The ynamic equations were simulate in MATLAB an the optimization was implemente using MATLAB s lsqnonlin function. Some of the planning results are presente below. A. Results of Optimal Shape Trajectory Planning This section presents a variety of esire x w (t) an y w (t) that satisfy the conitions in Sec. III-B an the corresponing planne optimal shape trajectories, φ p x(t) an φ p y(t), which shoul be tracke to achieve them. It is important to note that the esire trajectories, x w (t) an y w (t), are trajectories of the center of the ball an not trajectories of the system s center of gravity. On a flat floor, these trajectories match the trajectories of the ball s contact point with the floor. Fig.. color.) Straight Line Motion - Linear XY (This figure is best viewe in x w (t) = y w (t) = 9 a i t i, i= 9 b i t i, (6) i= where the coefficients a i s an b i s are etermine base on the initial an final esire configurations. Angle (eg) 5 Fig. 3. Time (s) φ p x φ p y Straight Line Motion - Planne Shape Trajectories

6 In Fig., we can see the planne XY motion approximately tracking the esire XY motion. The planne shape trajectories that prouce such a motion are shown in Fig. 3. The trajectories for the planne XY motion are obtaine from Eq. 8. The tracking error statistics are: RMSE =.47 m an Maximum Error =.38 m Desire Planne to be note that the results presente here are observe only when the appropriate initial conitions are met. The tracking error statistics are: RMSE = 4.3x 4 m an Maximum Error = 5.6x 4 m Desire Planne 3 3 Fig Figure-8 Motion - Linear XY (This figure is best viewe in color.) Fig. 4. Circular Motion - Linear XY (This figure is best viewe in color.) ) Circular Motion: Here, we woul like the robot to move along a circular path on the floor. The esire x w (t) an y w (t) are: x w (t) = Rsin(ωt), y w (t) = R( cos(ωt)), (7) where, R = m an ω = π/ ra/s. 3) Figure-8 Path: Here, we woul like the robot to move along a figure-8 path on the floor. The esire x w (t) an y w (t) are: x w (t) = A x sin(ω x t), y w (t) = A y sin(ω y t), (8) where, A x = 4 m, ω x = π/ ra/s, A y = 4 m, an ω y = π/ ra/s. Angle (eg) φ p x φ p y Angle (eg) φ p x φ p y 5 5 Fig. 5. Time (s) Circular Motion - Planne Shape Trajectories In Fig. 4, we can see the planne XY motion approximately tracking the esire XY motion. The planne shape trajectories that prouce such a motion are shown in Fig. 5. The trajectories for the planne XY motion are obtaine from Eq. 8. It is 3 4 Fig. 7. Time (s) Figure-8 Motion - Planne Shape Trajectories In Fig. 6, we can see the planne XY motion approximately tracking the esire XY motion. The planne shape trajectories that prouce such a motion are shown in Fig. 7. It is to be note that the results presente here are observe only when the appropriate initial conitions are met. The trajectories for

7 the planne XY motion are obtaine from Eq. 8. The tracking error statistics are: RMSE =.5 m an Maximum Error =.77 m Desire Planne 3 3 Fig. 8. Join-Circle-Leave Motion - Linear XY (This figure is best viewe in color.) 4) Join-Circle-Leave Motion: The tracking of a circle, sine wave an figure-8 paths require that the appropriate initial conitions are met. But in reality, we woul like to start at any ranom initial configuration an track a particular path on the floor. Here, we present the result of starting from rest, joining a circular path at a esire configuration, making.5 revolutions, leaving the circular path at a iametrically opposite point an coming to rest. This motion consists of 3 ifferent trajectories fuse together. The joining an leaving trajectories are nonic (9 th egree) polynomials that satisfy the initial an final configurations require to join an leave the circular path respectively. The nonic polynomials ensure that the first four erivatives of the fuse trajectories are continuous an not just piecewise continuous. φ p x φ p y In Fig. 8, we can see the planne XY motion approximately tracking the esire XY motion. The planne shape trajectories that prouce such a motion are shown in Fig. 9. The trajectories for the planne XY motion are obtaine from Eq. 8. The tracking error statistics are: RMSE =.67 m an Maximum Error =.86 m. B. Real-Time Planning For all the results presente in Sec. IV-A, the optimization tolerance values for both the resiual norm an parameter values were set to < 4. On a stanar Intel Core- Duo processor, the optimization implementation in MATLAB converges in < secon. This ensures that the optimal shape trajectory planner presente in this paper can be use for realtime planning on the robot. V. BALANCING AND TRACKING CONTROL The entire planning proceure presente in this paper assumes that there exists a balancing controller, similar to the one in 9, which has goo shape trajectory tracking performance. Given the balancing controller an the optimal shape trajectory planner, we can achieve goo approximate tracking of the esire external configuration trajectories. But it is to be note that this tracking is open-loop an with wrong initial conitions, we have no way to ensure approximate tracking of the esire external configuration trajectories. Moreover, while testing on the real robot, moeling uncertainties, unmoele ynamics, nonlinear friction effects an noise may prevent goo approximate tracking. Hence, we shoul have an external trajectory tracking controller, similar to the one in 3, to ensure better tracking. q s - q s c q s p Balancing Controller Tracking Controller Optimal Shape Trajectory Planner Shape-Accelerate Uneractuate Balancing Systems - q x q s q x Fig.. Control Architecture Angle (eg) Time (s) Fig. 9. Join-Circle-Leave Motion - Planne Shape Trajectories We propose to follow 3 in using the control architecture shown in Fig. for goo approximate tracking of esire external configuration trajectories (q x) on the real robot. The balancing controller tracks the esire shape trajectories (q s ), which are a sum of planne (q p s) an compensation (q c s) shape trajectories. The planne shape trajectories are given by the optimal shape trajectory planner, whereas, the compensation shape trajectories are provie by the tracking controller, which tries to compensate for the eviation of the external configuration trajectories from the esire ones.

8 In Fig., we show the result of the control architecture in Fig. tracking the external configuration trajectories corresponing to a circular motion starting with zero initial conitions, i.e., there is no initial linear velocity in the x irection, as in Fig. 4. We can see that the motion asymptotically converges to the circular motion, which shows the effectiveness of the tracking controller in compensating for the errors ue to wrong initial conitions Desire Actual 3 3 Fig.. Tracking Circular Motion with Zero Initial Conitions (This figure is best viewe in color.) The contribution of this paper is the optimal shape trajectory planner, whereas, the balancing an tracking controller esigns are borrowe from our previous work 3. In 3, we have shown that the control architecture in Fig. is capable of successfully tracking straight line motions on the real robot. Base on these previous successful experimental results, we strongly believe that this control architecture will enable the real robot to track the motions presente in Sec. IV-A. It is to be note that the planner in 3 was esigne base on our intuition of how the robot shoul lean in orer to achieve a straight line motion, whereas, in this paper, we have presente a generalize shape trajectory planner that will enable the ballbot to approximately track any esire external configuration trajectory. VI. CONCLUSION An algorithm to plan optimal shape trajectories for shapeaccelerate uneractuate balancing systems was presente. These shape trajectories, which when tracke will result in approximate tracking of the esire external configuration trajectories subject to conitions on ifferentiability of the esire trajectory an correct initial conitions. The planning proceure presente assumes that there exists a balancing controller that can accurately track esire shape trajectories. A feeback trajectory tracking controller use in combination with the optimal shape planner can ensure better tracking when the initial conitions are not met. In the 3D ballbot example, we have seen that given (x w(t),y w(t)) C 4, we can use the optimal shape trajectory planner to obtain the esire shape trajectories, which when tracke will result in the esire motion. In a general motion planning setting, any stanar path planning algorithm like A can be use to obtain a esire(x w (s),y w (s)) path. In orer to use the optimal shape trajectory planner iscusse in this paper as a tool for overall motion planning, we have to parametrize this path in time. We propose that using nonic splines, which ensure that the first four erivatives for the overall trajectory will be continuous throughout its omain, will help us achieve this goal. Future work will inclue experimental testing of the planne shape trajectories presente here an esign of a global motion planner that provies esire external configuration trajectories for the optimal shape trajectory planner. VII. ACKNOWLEDGEMENTS This work was supporte in part by NSF grants IIS-3867 an IIS The author thanks Ralph Hollis, George Kantor, Howie Choset an RSS reviewers for their valuable comments that helpe in improving this paper. REFERENCES M. W. Spong, The control of uneractuate mechanical systems, in First International Conference on Mechatronics, Mexico City, 994. H. G. Nguyen, J. Morrell, K. Mullens, A. Burmeister, S. Miles, N. Farrington, K. Thomas, an D. Gage, Segway robotic mobility platform, in SPIE Proc. 569: Mobile Robots XVII, Philaelphia, PA, October 4. 3 R. Hollis, Ballbots, Scientific American, pp. 7 78, October 6. 4 R. Olfati-Saber, Nonlinear control of uneractuate mechanical systems with application to robotics an aerospace vehicles, Ph.D. issertation, Massachusetts Institute of Technology, February. 5 M. Raibert, K. Blankespoor, G. Nelson, R. Playter, an the Big- Dog Team, Bigog, the rough-terrain quarape robot, in In Proc. 7th Worl Congress of the International Feeration of Automatic Control, 8, pp J. Grizzle, J. Hurst, B. Morris, H.-W. Park, an K. Sreenath, Mabel, a new robotic bipeal walker an runner, in American Control Conference, St. Louis, MO, June 9. 7 P. Deegan, B. Thiboeau, an R. Grupen, Designing a self-stabilizing robot for ynamic mobile manipulation, in Robotics: Science an Systems - Workshop on Manipulation for Human Environments, August 6. 8 J. R. Ray, Nonholonomic constraints, American Journal of Physics, vol. 34, pp , U. Nagarajan, A. Mampetta, G. Kantor, an R. Hollis, State transition, balancing, station keeping, an yaw control for a ynamically stable single spherical wheel mobile robot, Proc. IEEE Int l. Conf. on Robotics an Automation, May 9. A. Isiori, Nonlinear Control Systems. Berlin: Springer-Verlag, 989. A. Isiori an C. I. Byrnes, Output regulation of nonlinear systems, IEEE Transactions on Automatic Control, vol. 35, no., pp. 3 4, 99. N. H. Getz, Dynamic inversion of nonlinear maps with applications to nonlinear control an robotics, Ph.D. issertation, University of California at Berkeley, December U. Nagarajan, G. Kantor, an R. Hollis, Trajectory planning an control of an uneractuate ynamically stable single spherical wheele mobile robot, Proc. IEEE Int l. Conf. on Robotics an Automation, May 9. 4 G. Oriolo an Y. Nakamura, Control of mechanical systems with secon-orer nonholonomic constraints: Uneractuate manipulators, in In Proc. 3th IEEE Conference on Decision an Control, vol. 3, 99, pp