1
Comparison Between Two Actuation Schemes for Underactuated Brachiation Robots Vin´ıcius Menezes de Oliveira∗ Walter Fetter Lages† of Mathematics, FURG, Rio Grande, Brazil † Department of Electrical Engineering, UFRGS, Porto Alegre, Brazil ∗ Department
Abstract— In this paper we present a comparative study of two actuation schemes for underactuated brachiation robots, using the nonlinear model based predictive control (MPC) technique to the motion control. The actuation schemes are based on two underactuated robots, the Acrobot and the Pendubot, both with 2 links. However, in this work a robot with 3 links, i.e., two arms and a body, is used. We adopt the MPC to control the underactuated brachiation robot due to its advantages, mainly the construction of an optimal stabilizing control law and due to its ability to consider in a direct way the constraints into the optimization problem. At the end we present computational simulations with their respective results and we highlight some important considerations. Index Terms— Underactuated systems, brachiation robots, predictive control
I. I NTRODUCTION In the past decade, there has been an increase in the worldwide use of robotic systems in a large variety of automatized activities, in a conservative forecast there will be about 1 million robots working around the world at the end of this decade [23]. The most robotized economic activity is the automotive industry, where the assembly and soldery tasks are executed by robot manipulators. Robots are used in industry to execute repetitive and hard tasks in hazardous environments. However, the continuous growth of the number of robots used for service is considerable [18] and cannot be neglected. Innovative developments in the areas of sensor, control and drive make the implementation of intelligent robots possible in different applications beyond industrial production. There are various practical situations where one can use service robots, for example, gas station (refueling of vehicles), agriculture and forestry, construction industry, renovation of nuclear power plant, hobbies and recreation. Moreover, one activity that is getting the attention from researchers and robot manufacturers is the inspection task. Robots for inspection are already used in various sectors, such as nuclear power plants and pipes (electrical and gas). One of the inspection tasks that has received considerable research attention if the inspection of power transmission lines, where the traditional methods of inspection involve tedious and high cost procedures. Visual inspection executed by human operators over long extension of cables and a high number of isolators is undoubtedly a problem. Due to the repetitive scene of cables and isolators the inspection of such elements is monotonous and the quality of the inspection 1-4244-1264-1/07/$25.00 ©2007 IEEE
Fig. 1.
Brachiation of an ape from branch to branch.
result cannot be guaranteed. Many papers in the literature have considered the application of service robots for this task, but it is still an open subject for investigation [12, 22, 4, 1, 3, 8, 17]. Considering the inspection of power transmission lines, robots are already used in underground cable systems. Recently, in New York city, a robot to inspect underground cables was installed. In our work, we will adopt the same methods and procedures for cable inspection as the above. Nonetheless, this work proposes the development of a robot for inspection of overhead power transmission lines, instead of underground ones. The inspection of overhead wires implies a suitable locomotion system to move along the line and to avoid the fall of the robot. A significant amount of the project development time is dedicated to the study of different locomotion systems to overcome obstacles, such as aircraft warning spheres, counterweights and towers. In the beginning of the last decade a new class of mobile robot was proposed by Prof. Toshio Fukuda in [9], presenting a six-link full-actuated brachiation robot that imitates the movement of an ape swinging from branch to branch (figure 1). This robot makes effective use of gravity for swinging and move itself. In the sequel a simpler two-link brachiation robot was proposed [10], where a control system using CMAC (Cerebellar Model Arithmetic Computer) was developed, considering heuristic creation of input driving forces in a previous training. Moreover, this two-link robot was designed as an underactuated system, i. e., the robot has more degrees of freedom than control inputs. The control of underactuated mechanical systems is a research topic that during the past years has been in vogue in the literature [15, 16, 21, 13, 14]. The study of brachiation systems, specifically the underactuated ones, can be justified by situations where it is necessary to reduce the weight of the robot or when it is desired to maximize the time of autonomy of the robot (minimizing the energy consumption). Moreover, from the point of view of
PSfrag replacements 2
the control theory, underactuated mechanical systems present second order non-integrable differential constraints, commonly referred to as nonholonomic second order systems [15, 11]. The objective of this paper is to present a first study about two different actuation schemes (based on the well-known robots Acrobot [19] and Pendubot [20]) to the control of an underactuated brachiation robot intended to be used in the inspection of power transmission lines. In [6, 7] one can see our previous works. The control architecture used is the NMPC (Nonlinear Model-based Predictive Control) scheme because it can deal with constraints on inputs and state and generates an implicit optimal control law [5]. Also, our approach does not suppose the existence of a reference trajectory, but it considers that the robot must achieve the horizontal line, in a position as far as possible. Furthermore, bearing in mind the idea to take advantage of gravity forces to move the robot, it can be supposed that the optimal control would be one that makes effective use of such forces. Hence, the NPMC would generate a control that exploit the gravity forces. This paper is organized as follows. In section II is developed the dynamic model of the brachiation robot and presented the changes for the two actuation schemes and in section III the nonlinear model-based predictive control scheme is presented. In the sequel, in section IV we give some results from different initial positions comparing the actuation schemes proposed. Concluding, some discussion concerning the performance of each configuration used and some directions for further studies are carried out. II. DYNAMICS M ODELLING Considering the brachiation robot (seen in figure 2) as a serial open-chain robotic manipulator, its dynamics can be generally given by: ¨ + V (θ, θ) ˙ θ(t) ˙ + G(θ) = Bτ − Fv (θ) ˙ M (θ)θ(t)
(1)
where θ = [θ1 θ2 θ3 ] is the vector of the joints coordinates, M (θ) ∈ R3×3 is a symmetric matrix representing the inertia terms, given by1 : T
y0 x0 θ1 m1 l1
y2 y1
m2 l2
x2
θ2 m3 l3
θ3 x1 y3 x3
Fig. 2.
Underactuated brachiation robot.
The constants are defined as: l2 α1 , m1 1 + I1 + m2 l12 + m3 l12 4 l22 α2 , m 2 + I 2 4 l2 α 3 , m 2 l1 2 l32 α4 , m 3 + I 3 4 l3 α 5 , m 3 l1 2 l1 α 6 , m 1 + m 2 l1 + m 3 l1 2 l2 α7 , m 2 2 l3 α8 , m 3 2 The robot used in this work is designed as an underactuated system, i. e., a system in which the number of control inputs m is smaller than the number of degrees of freedom n (m < n). In other words, the dimension of the input vector τ ∈ Rm×1 is smaller than the dimension of the state vector θ ∈ Rn×1 . The input selection matrix B is a real matrix of dimension (n × m). The equations of the dynamics of the robot given by (1) can be rewritten in a more suitable state space form: q(t) ˙ = f (q) + g(q)u
(5)
˙ is a 6 × 1 vector of generalized coordinates and where q = [θ θ] α4 + α 5 c 3 " # θ˙ 0 h i f (q) = α4 −M −1 (q) V (q)θ˙ + G(q) + Fv (q) (2) ˙ ∈ R3×3 is a matrix with the Coriolis and centrifugal terms: V (θ, θ) and 03×2 g(q) = V = M −1 (q)B ˙ ˙ ˙ ˙ ˙ ˙ −α3 θ2 s2 − α5 θ3 s3 −α3 θ2 s2 − α3 θ1 s2 −α5 θ3 s3 − α5 θ1 s3 As mentioned before, in this paper we consider two different configurations for the actuator (as depicted in figure 3), both with 0 0 α3 θ˙1 s2 m = 1 control input, but with different actuated joints. The first 0 0 α5 θ˙1 s3 (3) configuration has the first joint actuated (Pendubot-like configuration) and the second one has the second joint actuated (Acrobot-like and G(θ) ∈ R3×1 is a vector with the gravity terms given by2 : configuration). The input selection matrix and the input vector are α6 gc1 + α7 gc12 + α8 gc13 defined for each configuration, respectively, as: α7 gc12 (4) G= • configuration 1 (only the first joint is actuated): α8 gc13 1 3×1 and Fv ∈ R represents the viscous friction terms. B = 0 0 1 The function cos(θ ) is indicated as c . The same holds for the sin() i i (6) function. 2 The term c τ = τ1 ij indicates cos(θi + θj ).
α1 + α2 + 2α3 c2 + α4 + 2α5 c3 α2 + α 3 c 2 M = α4 + α 5 c 3
α2 + α 2 c 2 α2 0
T
3
•
following form:
configuration 2 (only the second joint is actuated): 0 B = 1 0
Sfrag replacements
Φ(t)
=
(7)
Free Joint
xT (k + j|k)Qx(k + j|k)
N X
uT (k + j − 1|k)Ru(k + j − 1|k)
(10)
j=1
where N is the prediction and 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:
Free Joint
Free Joint
+
τ2
τ1
N X j=1
PSfrag replacements τ
=
Free Joint
x(k + j|k) u(k + j|k)
τ2
∈ ∈
X, U,
j ∈ [1, N ] j ∈ [0, N ]
where X is the set of all possible values for x and U is the set for all possible values for u. By supposing that such constraints are linear with respect to x and u, we can write: Fig. 3.
Two different actuation schemes considered.
Cx(k + j|k) Du(k + j|k)
III. MPC S CHEME The basic elements present in all model-based predictive controller are: prediction model, objective function and optimization procedure. 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 the prediction model, but in different MPC schemes, other models could be used [2]. 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. In the following we will develop the the NMPC scheme proposed in this work, represented by the block diagram in figure 4. Consider a general nonlinear model, expressed as: (8)
x(t) ˙ = f (x(t), u(t))
where x(t) is the state vector and u(t) is the control input vector. The nonlinear model, now described in discrete time, is given by: (9)
x(k + 1) = f (x(k), u(k))
The objective function to be minimized assumes, in general, the
Ref Past Inputs and Outputs
Fig. 4.
j ∈ [1, N ] j ∈ [0, N ]
(11) (12)
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 they minimize the objective function Φ(k) under imposed constraints, that is: u∗ , x∗ = arg min {Φ(k)} (13) u,x
subjected to: x(k|k) x(k + j|k)
= =
Cx(k + j|k) Du(k + j|k)
≤ ≤
x0 f (x(k + j − 1|k), u(k + j − 1|k)), j ∈ [1, N ] c, j ∈ [1, N ] d, j ∈ [0, N ]
(14) (15) (16) (17)
where x0 is the value of x in instant k. The problem of minimizing (13) is solved for each sampling time, resulting in the optimal control sequence: u∗ = {u∗ (k|k), u∗ (k + 1|k), . . . , u∗ (k + N |k)}
(18)
and the optimal state sequence is given by: x∗ = {x∗ (k + 1|k), . . . , x∗ (k + N |k)}
(19)
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)
x(δ) ˙ = f (x(δ), h(δ))
(20)
(21)
A. Brachiation Robot Control
Future Inputs
rag replacements u(k) x(k) xr (k) NMPC Robot
c, d,
where h(δ) is continuous during the sampling interval T . Hence, from the above, the closed-loop system reads:
Predicted Outputs
Nonlinear Model
≤ ≤
Future Errors Nonlinear Optimization
Objective Function
Constraints
General NMPC scheme considered in this work.
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 (5) can be discretized using Euler method and one can rewrite: q(k + 1) = q(k) + T f (k) + T g(k)u(k) (22) where T is the sampling interval. The objective function takes into account the cartesian position and velocity of the end-effector of the robot, instead of considering directly the joint coordinates, because the robot must reach the supporting line (y = 0) with null velocity, independently of the joint
4
configuration. It is important to highlight the fact that the robot is not fully actuated. Thus, the objective function is given by:
=
T
X (k + j|k)QX(k + j|k)
j=1
+
N X
˙ + j|k) X˙ T (k + j|k)RX(k
j=1
+
N X
uT (k + j − 1|k)Su(k + j − 1|k)
(23)
1 , −1
d=
τmax τmax
(24)
and
I C= , −I
θ c = max θmax
(25)
where τmax is maximum torque and θmax = π is the maximum angular displacement of each joint.
−2
Joint 1 1.5 1.0 0.1
• • •
• • •
For the horizontally stretched simulation, considering the Pendubot-like configuration, the angular position of each joint is depicted in figure 5 and the respective angular velocity is presented in figure 6.
12
dΘ
2
dΘ3
−5
0
2
4
6
Time (s)
8
10
12
Angular velocity of each joint (Pendubot-like configuration).
XY−Plan End Effector Trajectory
0
−0.5
−1
−1.5
prediction horizon N = 5; Q = diag(5, 10); R = diag(0.01, 0.01); S = 0.01; maximum joint displacement qmax = π (rd); maximum joint velocity q˙ = 20 (rd/s) maximum torque τmax = 30 (N m).
10
dΘ1
0.5
Joint 3 2.0 1.5 0.1
and the constraints values are:
8
0
Fig. 6.
For the simulation, the parameters of the controller used are: •
6
Time (s)
5
−10
Joint 2 1.5 1.0 0.1
4
10
Distance Y (m)
Unit Kg m Nm/s
2
Angular position of each joint (Pendubot-like configuration).
TABLE I DYNAMICAL PARAMETERS OF THE ROBOT. Parameter Mass Length Viscous Friction
0
15
IV. R ESULTS In this section we present the results obtained from simulation comparing the two different actuation schemes. The initial position considered poses the robot with the two arms horizontally stretched (q0 = [−π 0 π2 0 0 0]T ). c The simulation was implemented using the software Matlab , with 500 iteractions and a sampling time of 10ms and a control interval of 20ms. The dynamics parameters of the robot used in the simulations are shown in table I.
2
Θ3
0
Fig. 5.
Joint Velocity (rd/s)
D=
Θ
2
−4
j=1
where X = [x y]T is the cartesian coordinates vector, X˙ = [x˙ y] ˙T is the cartesian velocities vector, Q and R are 2 × 2 matrices and R is a real scalar. The NMPC deals with the system constraints as described by equations (11) and (12)
Θ
1
Joint Position (rd)
Φ(t)
N X
4
−2 −2
Fig. 7.
−1
0
1
Distance X (m)
2
3
4
End-effector position (Pendubot-like configuration).
The cartesian trajectory of the end effector is presented in figure 7, where one can observe the initial position of the robot and its position when the robot completes the first brachiation motion. The external torque applied on joint 1 can be observed in figure 8
5
Torque on Joint 1
0.5
3
0.4
Θ1 Θ2
2
Θ3
Joint Position (rd)
0.3
Torque (Nm)
0.2 0.1 0 −0.1
1 0 −1 −2
−0.2 −3
−0.3 −0.4
Fig. 8.
0
1
2
3
4
5
6
Time (s)
7
8
9
−4
10
Applied torque (Pendubot-like configuration).
Fig. 11.
0
2
4
6
8
Time (s)
10
12
Angular position of each joint (Acrobot-like configuration).
(rd/s)
Considering now the Acrobot-like with the horizontally stretched configuration, the cartesian trajectory of the end-effector is presented in figure 9 and the external torque applied on joint 2 can be observed in figure 10.
XY−Plan End Effector Trajectory
Velocity
0.5
−0.5
Joint
Distance Y (m)
0
−1
10
dΘ1
5
dΘ2 dΘ3
0 −5 −10
0
2
4
6
Fig. 12.
−2 −2
Fig. 9.
−1
0
1
Distance X (m)
2
3
4
8
Time (s)
−1.5
10
12
Angular velocity of each joint (Acrobot-like configuration).
5
End-effector position (Acrobot-like configuration).
The values of the objective function for each one of the configurations, considering the horizontally stretched position are depicted in figures 13 and 14, respectively to the ?? configuration and to the ?? configuration;
Torque on Joint 2
4
2 1 0
400
−1 −2 −3
Fig. 10.
Objective Function
500
Cost
Torque (Nm)
3
0
1
2
3
4
5
Time (s)
6
7
8
9
10
300 200 100
Applied torque on joint 2 (Acrobot-like configuration).
The angular position of each joint and the correspondent angular velocity can ben seen in figures 11 and 12.
0
Fig. 13.
0
1
2
3
4
5
Time (s)
6
7
8
Objective function (Pendubot-like configuration).
9
10
6
Objective Function
450 400 350
Cost
300 250 200 150 100 50 0
Fig. 14.
0
1
2
3
4
5
Time (s)
6
7
8
9
10
Objective function (Acrobot-like configuration).
V. C ONCLUSIONS In this work we have proposed a comparison study concerning two different actuation schemes based on the well-known underactuated robots Acrobot and Pendubot. It is used the NMPC technique to the motion control of our underactuated brachiation robot. According to the results presented in section IV, it is possible to observe that using the Acrobot-like actuation scheme the brachiation robots moves far from its initial position than using the Pendubot-like configuration. We have adopted the same controller parameters for both configurations to compare the results, but it is very important to highlight that it is not possible to say that the Acrobot-like configuration is better than the Pendubot-like configuration. Future directions of the work are the analysis of the robustness of the control scheme, considering the model mismatches and disturbances. Moreover, to the real-time implementation it is interesting to investigate the use of linearization of the dynamics to the optimization procedure.
ACKNOWLEDGMENTS The authors gratefully acknowledge the financial support from CAPES (Coordenac¸a˜ o de Aperfeic¸oamento de Pessoal de N´ıvel Superior), CNPq (Conselho Nacional de Desenvolvimento Cient´ıfico e Tecnol´ogico) and from CEEE (Companhia Estadual de Energia El´etrica).
R EFERENCES [1] R. Aracil, M. Ferre, M. Hernando, E. Pinto, and J. M. Sebastian, “Telerobotic system for live-power line maintenance,” Control Engineering Practice, vol. 10, no. 11, pp. 1271–1281, November 2002. [2] E. F. Camacho and C. Bordons, Model Predictive Control, ser. Advanced Textbooks in Control and Signal Processing. Springer-Verlag, 1999. [3] M. F. M. Campos, G. A. S. Pereira, S. R. C. Vale, A. Q. Bracarence, G. A. Pinheiro, and M. P. Oliveira, “A robot for installation and removal of aircraft warning spheres on aerial power transmission lines,” IEEE Transactions on Power Delivery, vol. 18, no. 4, pp. 1581–1582, October 2003. [4] J. Cˆot´e, S. Montambault, and M. St.-Loius, “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, October 2000, pp. 21–27. [5] S. L. de Oliveira, “Model predictive control for constrained nonlinear systems,” Ph.D. dissertation, California Institue of Technology (CALTECH), California - USA, 1996. [6] V. M. de Oliveira and W. F. Lages, “Predictive control of an underactuated brachiation robot,” in Proceedings of the 7th Portugueses Control Conference on Automatic Control, September 2006. [7] V. M. de Oliveira and W. F. Lages, “Predictive control of an underactuated brachiation robot using linearization,” in Proceedings of the 8th International IFAC Symposium on Robot Control, September 2006. [8] A. de Souza, L. A. Moscato, M. F. dos Santos, W. de Brito Vidal Filho, G. A. N. Ferreira, and A. G. Ventrella, “Inspection robot for high-voltage transmission lines,” in ABCM Symposium Series in Mechatronics, A. B. of Mechanical Sciences and Engineering, Eds., vol. 1, 2004.
[9] T. Fukuda, H. Hosokal, and Y. Kondo, “Brachiation type of mobile robot,” in International Conference on Advanced Robotics, ser. Robots in Unstructured Environments, vol. 2, June 1991, pp. 915–920. [10] T. Fukuda, F. Saito, and F. Arai, “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, November 1991, pp. 478–483. [11] A. D. Luca and S. Iannitti, “A simple stlc test for mechanical systems underactuated by one control,” in Proceedings of the IEEE International Conference on Robotics & Automation, May 2002, pp. 1735–1740. [12] Y. Maruyama, K. Maki, and H. Mori, “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, September 1993. [13] J. Nakanishi, T. Fukuda, and D. E. Koditschek, “Brachiation on a ladder with irregular intervals,” in Proceedings of the 1998 IEEE Internation Conference on Robotics and Automation, Detroit, Michigan, May 1999, pp. 2717 – 2722. [14] J. Nakanishi, T. Fukuda, and D. E. Koditschek, “A hybrid swing up controller for a two-link brachiating robot,” in Proceedings of the 1999 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Atlanta, USA, September 1999, pp. 549 – 554. [15] G. Oriolo and Y. Nakamura, “Control of mechanical systems with second-order nonholonomic constraints: Underactuated manipulators,” in Proceedings of the 30th Conference on Decision and Control, December 1991, pp. 2398–2403. [16] M. Reyhanoglu, A. van der Schaft, N. H. McClamroch, and I. Kolmanovsky, “Nonlinear control of a class of underactuated systems,” in Proceedings of the 35th Conference on Decision and Control, December 1996, pp. 1682–1687. [17] J. Rocha and J. ao Siqueira, “New approaches for surveillance tasks,” in 5th IFAC/EUROn SYmposium on Intelligent Autonomous Vehicles,
[email protected], 2004. [18] R. D. Schraft and G. Schmierer, Service Robots. A. K. Peters, 2000. [19] M. W. Spong, “Swing up control for the acrobot,” in Proceedings of the IEEE International Conference on Robotics and Automation, vol. 3, May 1994, pp. 2356–2361. [20] M. W. Spong and D. J. Block, “The pendubot: A mechanic system for control research and education,” in Proceedings of the 34th Conference on Decision and Control, December 1995, pp. 555–556. [21] C.-Y. Su and Y. Stepanenko, “Adaptive variable structure set-point control of underactuated robots,” IEEE Transactions on Automatic Control, vol. 44, no. 11, pp. 2090–2093, November 1999. [22] S. Tanaka, Y. Maruyama, K. Yano, H. Inokuchi, T. Torayama, and S. Murai, “Development of a hot-line work robot, ”phase ii” and a training system for robot operators,” in International Conference on Transmission & Distribution Construction Operation & Live-Line Maintenance, April 1998, pp. 147–153. [23] UNECE, “Worldwide growth in the period 2004-2007,” United Nations Economic Commission for Europe - World Robotics, Tech. Rep., October 2004.