predictive control of an underactuated brachiation

PREDICTIVE CONTROL OF AN UNDERACTUATED BRACHIATION ROBOT Vin´ıcius Menezes de Oliveira ∗,1 Walter Fetter Lages ∗∗,1

∗ Mathematics Department Federal University of Rio Grande Rio Grande, Brazil [email protected] ∗∗ Electrical Engineering Department Federal University of Rio Grande do Sul Porto Alegre, Brazil [email protected]

Abstract: This work is focused on the position control of an underactuated brachiation mobile robot with 3 links. We present the modeling of the dynamics of the robot and introduce the application of the nonlinear model-based predictive control (NMPC) to such a system. The robot has 3 revolute joints but only one of them is actuated, i. e., the robot has less control inputs than degrees of freedom. The investigation of the NMPC to control such a system is due to the fact that NMPC is able do deal with constraints (state or input limitations) in a direct way, obtaining an optimal control law. Simulation results are shown, as well as some c directions for future developments. Copyright IFAC 2006 Keywords: brachiation mobile robot, nonlinear control, underactuated systems, NMPC

1. INTRODUCTION In the beginning of the past decade a new type of mobile robot was proposed in (Fukuda et al. 1991b), presenting a six-link full-actuated brachiation mobile robot that imitates the movement of an ape swinging from branch to branch. This new type of mobile robot effectively used the gravity for swinging, instead of just compensate for it to reduce its influence. In the sequel a simpler two-link brachiation robot was proposed in (Fukuda et al. 1991a), where a control system using CMAC (Cerebellar Model Arithmetic Computer) was proposed, considering a heuristic 1

The authors gratefully acknowledge the financial support from CAPES and CEEE.

creation of input driving forces in a previous training. Moreover, that two-link robot was designed as an underactuated system, i. e., the robot has two degree of freedom and only one control input, so it can move only dynamically. The control of underactuated mechanical systems is a research topic that during the past years has been in vogue in the literature, for example, (Oriolo and Nakamura 1991, Reyhanoglu et al. 1996, Reyhanoglu et al. 1999, Su and Stepanenko 1999). In (Saito et al. 1994) the feedback control also considered the direction of the arm to the target. Another important work is (Nishimura and Funaki 1996, Nishimura and Funaki 1998), where an error learning method of the final-state control is applied to an underactuated robot. That

y0 paper considered a linearized state-space formulaPSfrag replacements x0 tion with time-varying parameters. In (Nakanishi et al. 1997, Nakanishi et al. 1998, Nakanishi et θ1 al. 2000), biomechanics arguments are considered, leading to three distinct brachiation movements, l1 , m 1 y2 depending on the goal of the task. Those papers y 1 used a control strategy to track some target dyl2 , m 2 x2 namics which describe the task through inputoutput linearization. A fuzzy controller approach θ2 was presented in (Hasegawa et al. 1999). A more l3 , m 3 x1 detailed analysis considering an underactuated θ3 brachiation mobile robot is presented in (Odagaki y3 et al. 1997). The paper (Gomes and Ruina 2005) x3 approaches the minimum-muscle-work, developing solutions that do not require external joint Fig. 1. Three-link brachiation robot. drive. robot has two arms and one link acting like a body where payload can located. Each arm has The robot proposed here is intented to be used in a gripper that can attach firmly to the supporting the inspection of power transmission lines. There line, allowing the robot to execute its movement are several papers in literature developing robots in a way similar to a manipulator robot. It moves for such task, but it is still open research topic. by releasing one arm from the supporting line One can observe all phases of a project, since and grasping it again in a forward point. It is the design of a remotely manually operated robot important to keep in mind that although the robot till a more sophisticated solution (Maruyama et has three joints, only one of them (joint 2) is al. 1993, Tanaka et al. 1998). Similar work, with actuated. different solutions, are described in (Côté et al. 2000, Aracil et al. 2002, Campos et al. 2003, As usual, the coordinate systems attached to the de Souza et al. 2004, Rocha and Siqueira 2004). robot shown in figure 1 are determined by the The main advantage of the robot proposed in this Denavit-Hartenberg convention. Considering this paper is its capability to overcome obstacles on the brachiation robot as a serial open-chain robotic supporting line due to its locomotion structure. manipulator, its dynamics can be generally given by (Fu et al. 1987): In this paper we apply the NMPC scheme to an underactuated brachiation mobile robot, with ¨ + H(θ, θ) ˙ + G(θ) = τ − Fv D(θ)θ(t) (1) three-links: two arms and a body. The NMPC scheme is adopted because it can deal with conParticularly, for our brachiation robot, D(θ) is an straints and system limits directly during the con3 × 3 symmetric matrix representing the inertia, trol computation (Camacho and Bordons 1999) whose elements are: and generates an implicit optimal control law. Also, our approach does not suppose the existence 4m2 l2 m1 l 2 + + m2 l2 cos(θ2 ) + d = 11 of a reference trajectory. Accordingly with the 3 3 idea of take advantage of gravity forces to move 4m3 l2 + + m3 l2 cos(θ3 ) the robot, it can be supposed that the optimal 3 control would be one that makes effective use of m2 l 2 m2 l2 cos(θ2 ) such forces. Hence, the NPMC would generate a d12 = + 3 2 control that exploit the gravity forces. m3 l 2 m3 l2 cos(θ3 ) d13 = + This paper is organized as follows. In section 2 is 3 2 developed the dynamic model of the brachiation robot and in section 3 the nonlinear model-based m2 l 2 m2 l2 cos(θ2 ) predictive control scheme is presented. In the d21 = + 3 2 sequel (section 4) some results of simulation are m2 l 2 presented. Discussion concerning the results and d22 = the control scheme used and some conclusions are 3 given in section 5. d23 = 0

2. UNDERACTUATED BRACHIATION ROBOT DYNAMICS The 3-link underactuated brachiation mobile robot considered in this work is shown in figure 1. This

m3 l2 cos(θ3 ) m3 l 2 + 3 2 d32 = 0 m3 l 2 d33 = 3 d31 =

˙ is a nonlinear Coriolis and centrifugal H(θ, θ) force vector whose elements are: 2

m2 l2 sin(θ2 )θ˙2 − m2 l2 sin(θ2 )θ˙1 θ˙2 + 2 2 m3 l2 sin(θ3 )θ˙3 − − m3 l2 sin(θ3 )θ˙1 θ˙3 2 2 m2 l2 sin(θ2 )θ˙1 h2 = 2 2 2 m3 l sin(θ3 )θ˙1 h3 = 2

h1 = −

the inputs for the current sampling interval are used and the same procedure is repeated at the next sampling time. This mechanism is known as moving or receding horizon strategy, in reference to the way in which the time window shifts forward from one sampling time to the next one. In this section we present the equations of the nonlinear model predictive control (NMPC) and then the application to the control of an underactuated brachiation mobile robot.

G(θ) is the gravity loading force vector whose elements are:

3.1 NMPC Problem Formulation

τ is the torque on joints and Fv is the diagonal matrix with of viscous friction coefficients.

The basic elements present in all model-based predictive controller are: prediction model, objective function, calculation of the control action. The prediction model is the central part of the MPC, because it is important to predict the future outputs of the system. In this scheme, the state space model is used as prediction model, but in different MPC schemes, other models could be used (Camacho and Bordons 1999). The objective function defines the criteria to be optimized in order to force the generation of a control sequence that drives the system as desired.

The equations of the dynamics of the robot given by equation 1 can be rewritten in a more suitable state space form:

In the following we will develop the the NMPC scheme proposed in this work. Consider a general nonlinear model, expressed as:

q˙ = f (q) + g(q)u

(2)

x(t) ˙ = f (x(t), u(t))

(3)

where x(t) is the state vector and u(t) is the control input vector. The same nonlinear model, now described in discrete time, is given by:

m1 gl cos(θ1 ) m2 gl cos(θ1 + θ2 ) g1 = + + 2 2 m3 gl cos(θ1 + θ3 ) + m2 gl cos(θ1 ) + + m3 gl cos(θ1 ) 2 m2 gl cos(θ1 + θ2 ) g2 = 2 m3 gl cos(θ1 + θ3 ) g3 = 2

˙ T is the state vector and where q = [θ θ] θ˙ f (q) = −D(q)−1 (H(q) + G(q) + Fv ) and

0 g(q) = D(q)−1

x(k + 1) = f (x(k), u(k)) (4)

The vector of the applied torque is given by u = P τ with P = [0 1 0], which indicates that only the joint 2 is actuated.

(5)

(6)

The objective function to be minimized assumes, in general, the following form:

Φ(k) =

N X

xT (k + j|k)Qx(k + j|k)

N X

uT (k + j − 1|k)Ru(k + j − 1|k) (7)

j=1

3. NON-LINEAR MODEL PREDICTIVE CONTROL

+

j=1

Model predictive control is an optimal control strategy that uses the model of the system to obtain an optimal control sequence by minimizing an objective function. At each sampling interval, the model is used to predict the behavior of the system over a prediction horizon. Based on these predictions, an objective function is minimized with respect to the future sequence of inputs, thus requiring the solution of a constrained optimization problem for each sampling interval. Although prediction and optimization are performed over a future horizon, only the values of

where N is the prediction horizon, which here is the same as the control horizon and Q ≥ 0 and R ≥ 0 are weighting matrices that penalize the state error and the control effort, respectively. Considering the fact that every real system is in practice subjected to some constraint (for example physical limits), we define the following general constraint expressions: x(k + j|k) ∈ X ,

j ∈ [1, N ]

u(k + j|k) ∈ U,

j ∈ [0, N ]

where X is the closed and convex set of all possible values for x and U is the closed and convex set for all possible values for u. By supposing that such constraints are linear with respect to x and u, we can write: Cx(k + j|k) ≤ c,

j ∈ [1, N ]

(8)

Du(k + j|k) ≤ d,

j ∈ [0, N ]

(9)

Thus, the optimization problem, to be solved at each sample time k, is to find a control sequence u? and a state sequence x? such that minimize the objective function Φ(k) under imposed constraints, that is: u∗ , x∗ = arg min {Φ(k)} u,x

Φ(k) =

N X

X T (k + j|k)QX(k + j|k)

N X

uT (k + j − 1|k)Ru(k + j − 1|k)(20)

j=1

+

j=1

(10)

where X = [x y]T is the cartesian coordinates vector, Q is a 2 × 2 matrix and R is a real scalar.

(11)

The NMPC deals with the system constraints as described by equations 8 and 9. Besides the fact that the system is underactuated, the control input has upper and lower bounds expressed by

subjected to: x(k|k) = x0

instead of considering directly the joint coordinates, because the robot must reach the supporting line (y = 0), independently of the joint configuration. It is important to highlight the fact the robot is not fully actuated. Thus, the objective function is given by:

x(k + j|k) = f (x(k + j − 1|k), u(k + j − 1|k)), j ∈ [1, N ]

(12)

Cx(k + j|k) ≤ c,

j ∈ [1, N ]

(13)

Du(k + j|k) ≤ d,

j ∈ [0, N ]

(14)

−τmax ≤ τ ≤ τmax

with τmax = 30N m. Moreover, we have restricted each joint angle to be within the interval: −

where x0 is the value of x in instant k. The problem of minimizing 10 is solved for each sampling time, resulting in the optimal control sequence: u = {u (k|k), u (k+1|k), . . . , u (k+N |k)} (15) ∗

∗

∗

(21)

4π 4π ≤ θi ≤ 3 3

(22)

4. SIMULATION RESULTS

∗

and the optimal state sequence is given by: x∗ = {x∗ (k|k), x∗ (k+1|k), . . . , x∗ (k+N |k)} (16) with an optimal cost Φ∗ (k). Thus, the control law defined by NMPC is implicitly given by the first term of the optimal control sequence: h(δ) = u∗ (k|k)

(17)

where h(δ) is a continuous variable whose value is held constant during the sampling interval T . Hence, from the above, the closed-loop system becomes: x(δ) ˙ = f (x(δ), h(δ)) (18)

We initially posed the robot with θ1 = −π, θ2 = 0 and θ3 = 0 with null velocities. In this work we simulate the robot moving from the initial position to a fixed target position, such that the final position is in advance along X axis and the end effector achieves the supporting line y = 0. The prediction horizon N = 5 is used, with a sample interval T = 0.01s. The gain matrix Q is set: 1 0 Q= (23) 0 100 and R = 0.1.

3.2 Brachiation Mobile Robot Control According to the equations of the NMPC, it is necessary to describe the dynamics of the system in discrete time. Thus, the system given by 2 can be discretized using Euler method giving: q(k + 1) = q(k) + T f (k) + T g(k)u(k)

The objective of this work is to investigate the use of NMPC to control an underactuated brachiation mobile robot. The robot moves switching the arms as a monkey does in the branch transfer motion.

(19)

where T is the sampling interval. The objective function takes into account the cartesian position of the end-effector of the robot,

Figure 2 shows the angular position of each joint of the robot along the movement and in figure 3 the angular velocity of each joint. The position of the end effector of the robot along the X and Y axes are depicted in figure 4 and the trajectory executed by the end effector in the XY-plan can be viewed in figure 5. The objective function values are depicted in figure 6. The unique torque applied to the system is on joint 2 and is shown in figure 7.

Joint Position

1

Torque on Joint 2

0.6

0.5 0.4 0

0.2

Torque (Nm)

Angle (rad)

−0.5

−1

−1.5

−2

Θ1 Θ2 Θ3

−2.5

0

−0.2

−0.4 −3

−3.5

0

0.2

0.4

0.6

Time (s)

0.8

1

1.2

−0.6

1.4

Fig. 2. Angular position of each joint.

0.2

0.4

0.6

Time (s)

0.8

1

1.2

1.4

Fig. 7. Torque applied on joint 2. where m is the executed steps and k · k is the euclidian norm.

Joint Velocity

5

0

dΘ 1 dΘ 2 dΘ3

4

3

Table 1 shows influence of the prediction horizon in controler performance index , for the execution of a single movement.

Angle (rad/s)

2

1

0

−1

Table 1. Prediction horizon influence.

−2

−3

−4

0

0.2

0.4

0.6

0.8

Time (s)

1

1.2

1.4

Fig. 3. Angular velocity of each joint. X and Y Positions along time

2 Pos. X Pos. Y 1.5

1

Position (m)

0.5

Prediction Horizon

1 2 3 4 5 10 15

2,4143 2,3061 2,2524 2,1376 2,1614 1,4908 1,6459

0

−0.5

−1

5. CONCLUSION AND FUTURE WORK

−1.5

−2

0

0.5

1

Time (s)

1.5

2

2.5

This work is a first step towards the application of the Nonlinear Model-based Predictive Control (NMPC) to control an underactuated brachiation mobile robot with 3 links. According to the results of the previous section, it is possible to observe that the robot is able release its hand from the supporting line and grasp it again in a forward point. Currently further studies are being carried out to allow the robot to execute various cycles of motion, swinging and moving forward. Special attention must be taken to consider the gravitational force to reduce the external torque applied to the system, in order to maximize the time of autonomy of the robot. A real-time implementation is also planned.

Fig. 4. Trajectory on X and Y axes. Trajectory on XY Plan

0.5

0

Y (m)

−0.5

−1

−1.5

−2 −2

−1.5

−1

−0.5

0 X (m)

0.5

1

1.5

2

Fig. 5. Trajectory on XY-plan. Cost

800

700

600

500

REFERENCES

400

300

200

100

0

0

0.2

0.4

0.6

Time (s)

0.8

1

1.2

1.4

Fig. 6. Objective function. An usual metric to assess the controller performance is given by; m m 1 X 1 X x(k) − xref (k)

k˜ x(k)k = =

y(k) − yref (k) m m k=0

k=0

(24)

Aracil, R., M. Ferre, M. Hernando, E. Pinto and J. M. Sebastian (2002). Telerobotic system for live-power line maintenance. Control Engineering Practice 10(11), 1271–1281. Camacho, Eduardo F. and Carlos Bordons (1999). Model Predictive Control. Advanced Textbooks in Control and Signal Processing. Springer-Verlag. Campos, Mário F. M., Guilherme A. S. Pereira, Samuel R. C. Vale, Alexandre Q. Bracarence, Gustavo A. Pinheiro and Maur´ıcio P. Oliveira (2003). A robot for installation and removal

of aircraft warning spheres on aerial power transmission lines. IEEE Transactions on Power Delivery 18(4), 1581–1582. Côté, Jacques, Serge Montambault and MIchel St.-Loius (2000). Preliminary results on the development of a teleoperated compact trolley for live-line maintenance. In: IEEE Conference on Transmission and Distribution Construction, Operation and Live-line Maintenance. pp. 21–27. de Souza, Adinan, Lucas Antonio Moscato, Melquisedec Francisco dos Santos, Walter de Brito Vidal Filho, Gustavo André Nunes Ferreira and Armindo Gustavo Ventrella (2004). Inspection robot for high-voltage transmission lines. In: ABCM Symposium Series in Mechatronics (ABCM BrazilianSociety of Mechanical Sciences and Engineering, Eds.). Vol. 1. Fu, K. S., R. C. Gonzalez and C. D. G. Lee (1987). Robotics: Control, Sensing, Vision and Intelligence. McGraw-Hill. Fukuda, Toshio, Fuminori Saito and Fumihito Arai (1991a). A study on the brachiation type of mobile robots (heuristic creation of driving input and control using cmac). In: IEEE/RSJ International Workshop on Intelligent Robots and Systems. Vol. 2. pp. 478–483. Fukuda, Toshio, Hidemi Hosokal and Yuji Kondo (1991b). Brachiation type of mobile robot. In: International Conference on Advanced Robotics. Vol. 2 of Robots in Unstructured Environments. pp. 915–920. Gomes, Mario W. and Andy L. Ruina (2005). A five-link 2d brachiating ape model with lifelike motions and no energy cost. Hasegawa, Yasuhisa, Toshio Fukuda and Koji Shimojima (1999). Self-scaling reinforcement learning for fuzzy logic controller - applications to motion control of two-link brachiation robot. IEEE Transactions on Industrial Electronics 46(6), 1123–1131. Maruyama, Yoshinaga, Kenji Maki and Hironori Mori (1993). A hot-line manipulator remotely operated by the operator in the ground. Sixth International Conference on Transmission and Distribution Construction and Live Line Maintenance pp. 437–444. Nakanishi, Jun, Toshio Fukuda and Daniel E. Koditschek (1997). Preliminary studies of a second generation brachiation robot controller. In: Proceedings of the 1997 IEEE Internation Conference on Robotics and Automation. Albuquerque, New Mexico. pp. 2050 – 2056. Nakanishi, Jun, Toshio Fukuda and Daniel E. Koditschek (1998). Experimental implementation of a ”target dynamics” controller on a two-link brachiating robot. In: Proceedings of the 1998 IEEE Internation Conference on

Robotics and Automation. Leuven, Belgium. pp. 787 – 792. Nakanishi, Jun, Toshio Fukuda and Daniel E. Koditschek (2000). A brachiating robot controller. IEEE Transactions on Robotics and Automation 16(2), 109 – 123. Nishimura, Hidekazu and Koji Funaki (1996). Motion control of brachiation robot by using final-state control for parameter-varying systems. In: Proceedings of the 35th Conference on Decision and Control. Kobe, Japan. pp. 2474 – 2475. Nishimura, Hidekazu and Koji Funaki (1998). Motion control of three-link brachiation robot by using final-state control with error learning. IEEE/ASME Transactions on Mechatronics 3(2), 120 – 128. Odagaki, Hiroshi, Antonio Moran and Minoru Hayase (1997). Analysis of the dynamics and nonlinear control of under-actuated brachiation robots. Symposium of Instrumentationand-Control Engineers pp. 1137 – 1142. Oriolo, Giuseppe and Yoshihiko Nakamura (1991). Control of mechanical systems with secondorder nonholonomic constraints: Underactuated manipulators. In: Proceedings of the 30th Conference on Decision and Control. pp. 2398–2403. Reyhanoglu, Mahmut, Arjan van der Schaft, N. Harris McClamroch and Ilya Kolmanovsky (1996). Nonlinear control of a class of underactuated systems. In: Proceedings of the 35th Conference on Decision and Control. pp. 1682–1687. Reyhanoglu, Mahmut, Arjan van der Schaft, N. Harris McClamroch and Ilya Kolmanovsky (1999). Dynamics and control of a class of underactuated mechanical systems. IEEE Transactions on Automatic Control 44(9), 1663–1671. Rocha, José and João Siqueira (2004). New approaches for surveillance tasks. In: 5th IFAC/EUROn SYmposium on Intelligent Autonomous Vehicles. [email protected]. Saito, Fuminori, Toshio Fukuda and Fumihito Arai (1994). Swing and locomotion control for two-link brachiation robot. IEEE Control Systems Magazine 14(1), 5–12. Su, Chun-Yi and Yury Stepanenko (1999). Adaptive variable structure set-point control of underactuated robots. IEEE Transactions on Automatic Control 44(11), 2090–2093. Tanaka, Shinya, Yoshinaga Maruyama, Kyiji Yano, Hirofumi Inokuchi, Thosihide Torayama and Shinji Murai (1998). Development of a hot-line work robot, ”phase ii” and a training system for robot operators. In: International Conference on Transmission & Distribution Construction Operation & LiveLine Maintenance. pp. 147–153.