Auton Robot (2006) 21:199–210 DOI 10.1007/s10514-006-7275-9
Equilibrium analysis of multi-limbs walking and climbing robots Matteo Zoppi · Rezia Molfino
Published online: 22 April 2006 C Springer Science + Business Media, LLC 2006
Abstract The paper discusses the complex problem of assessing online the static equilibrium of staticallyindeterminate climbing and walking robots (CLAWARs) with quasi-static locomotion. The method proposed is general and works for whatever number of legs and ropes operated by actuated winches connecting the robot to the environment. The configuration of the robot is assigned. First, the compliance of the robot body, of the legs and the compliances of the ground and the ropes are modeled as localized elasticities. The static equilibrium problem of the resulting model is statically-determinate under the hypothesis that the foot and rope points (where the ropes are fixed to the robot body) are joined to the ground by bilateral constraints. Since these constraints are unilateral (the feet are contact points and can detach from the ground, and the ropes can become slack), it is necessary to apply an iterative solving procedure in order to solve the static equilibrium problem. The method presented in the paper is a fast and effective alternative to nonlinear analysis of a finite element model of the robot at any assigned configuration. As an example, we consider the case of the heavy-duty CLAWAR Roboclimber. Keywords Climbing and walking robots . Quasi-static locomotion . On-line equilibrium . Robot equilibrium modeling . Robots with ropes/tethers/strings M. Zoppi () · R. Molfino PMAR Robot Design Research Group, University of Genova, Via all’Opera 15A, 16145, Genoa, Italy e-mail:
[email protected] Tel.: +39 010 353 2837 Fax: +39 010 353 2298 R. Molfino e-mail:
[email protected]
1. Introduction Legs are a flexible and effective mean to move on uneven surfaces and natural environments, since they can easily adapt to the geometry of the terrain and in principle can provide support also on very steep surfaces. Insects move almost everywhere, they are able to overcome steep walls and are successful also on unstable and soft materials like sand or mud. Legged robots are promising systems but their engineering is always challenging and their design and control in order to achieve robust locomotion can be complex. The technical literature proposes many examples of CLAWARs (CLimbing and WAlking Robots) with four, six and more legs. Some recent prototypes are RHex (Saranli et al., 2001), SILO4 (Santos et al., 2003), Lauron III (Scholl et al., 2001), Dante II (Apostolopoulos and Bares, 1995; Krishna et al., 1997; Wettergreen et al., 1995). Most of these systems adopt quasi-static locomotion (Song and Waldron, 1989). When the number of legs is high, this is a natural choice for many reasons. The gait is a sequence of static-equilibrium configurations and this results in high robustness and intrinsic safety and an easy adaptability to the local geometry of the terrain. Moreover, for heavy systems, the actuation power that would be required for dynamic locomotion is higher than the one really achievable with conventional actuators. The control of the gait is easier than for dynamic gaits and in some cases to be “quasi-static” is an intrinsic property of the gait scheme adopted, e.g., double-tripod gait (Ting et al., 1994), without any need of assessing the robot equilibrium at the different gait configurations. For specific applications, in order to move on very steep surfaces and in presence of high force interaction with the
Springer
200
environment (Scholl et al., 2001; Hirose et al., 1997; The roboclimber partnership, 2003), the robot can be equipped with ropes anchored to the environment and operated by actuators placed onboard (such as winches). The consolidation of rocky walls (Hirose et al., 1997; The roboclimber partnership, 2003) is an example of task requiring a robot with ropes. In these cases the gait is planned online and it is necessary to check online the static equilibrium of the robot at every phase of the gait. A lack of static equilibrium can result in very harmful effects like overturning. A robot with a high number of legs and ropes is a statically-indeterminate system under unilateral constraints (at the ropes and ground contact points). To solve the static equilibrium problem it is necessary to take into account the mechanical compliance of the robot and the compliances of the ground and the ropes. This can be very complex and time-consuming. The methods proposed in the technical literature and applied to legged robots are for legged robots without ropes and most of them are based on strong approximations. The main reason why ropes are not considered is that so far only few roped robots have been proposed, and very few heavy-duty prototypes realized, mainly: TITAN VII (Hirose et al., 1997) (and the following development of the TITAN VII-concept by the research group of professor Hirose), DANTE II (Apostolopoulos and Bares, 1995) and Roboclimber (The roboclimber partnership 2003; Molfino et al., 2005; Cepolina et al., 2006). The interest in robots using ropes and tethers is growing; new concepts are
r hyper-tethered robots (Fukushima et al., 2001), i.e. systems of robots connected by tethers enhancing their cooperative capabilities, and r locomotion using launching and anchoring of tethers (Hirose and Fukushima, 2002). This may result in an increasing interest in effective methods for the modeling and analysis of robots connected to the environment or to other robots by ropes/tethers. The present paper proposes a fast and general method to solve the static equilibrium problem for CLAWARs with any number of legs and ropes. The background of this research is in several works proposed in the scientific literature, dealing with the representation of the stiffness at a contact point between two bodies. The pioneer work on the subject is Dimentberg (1965). The earlier paper discussing a contact model with preloaded springs is Ghafoor and Kerr (1992). The research is deepened in Griffis and Duffy (1993), where the stiffness matrix is obtained by referring to a system of three loaded line-springs. A generalization of the method can be found in Ciblak and Lipkin (1994). The subject is also further investigated in Ciblak (1998), Ciblak and Lipkin (1998), Zefran et al. (1999), Zefran and Kumar (2002) and Ghafoor Springer
Auton Robot (2006) 21:199–210
et al. (2004) dealing with robotics applications. Applications of part of these research results in static and dynamic analysis of multi-link systems, mainly for automotive, have been proposed and also software tools developed, e.g. (Hiller et al., 1997). Static and sometimes dynamic modeling of statically undetermined legged robots by means of compliances has been already addressed in the literature, e.g. Schneider and M¨uller 1999, and Schneider et al. (2000). In the present paper, instead of introducing systems of elastic elements, the contacts of the robot feet with the ground are described with reference to an elastic contact model from which we obtain the contact stiffness matrices, as discussed in the following. The robot is imagined as a rigid body suspended on compliant supports with suitable stiffness (equivalent to the real ropes, legs, frame and terrain stiffness), placed at the hip points (where the legs are joined to the robot body) and at the rope points (where the ropes are fixed to the robot body). The stiffness of the supports is estimated as a function of the structural and functional compliance of the parts composing the robot (body, legs and rope winches, actuation and control system) and on the elastic properties of the terrain. These stiffness values are computed analytically or mapped off-line for a sufficiently dense set of robot configurations, depending on the characteristics of the robot under analysis. The mapping operation can be performed by means of the analysis of a finite element (FE) model of the robot. The stiffness analysis campaign can be very time-consuming, but it is done once for all and off-line. The proposed approach is based on the main hypotheses:
r the reaction at every foot contact point is a pure force without moment component; this depends on the architecture of the legs and in particular of the feet r the error coming from representing the robot body as a rigid body and the leg and ground compliances as linear elasticities at the hip and rope supports is small r the displacements are supposed small. The second hypothesis is critical. On one hand, it is satisfied if the system (robot and ground) has an almost linear force-displacement behavior for small displacements. On the other hand, the legs and the rope winches are supposed to be the most compliant parts of the robot (the robot body is considered rigid). This is true in the most cases, since normally the body is a structural frame and the legs are serial mechanisms with two or more degrees of freedom, with a compliance coming from the links, the joints and the actuators. The compliance of the active mechanisms depends also on the control scheme adopted. Moreover, it is supposed that the feet do not slip on the ground and nonlinearity due to friction cones is neglected. This hypothesis is easily satisfied with a suitable design of the feet, e.g., sharp steel cones for moving on rocks.
Auton Robot (2006) 21:199–210
201
Fig. 1 The physical prototype of Roboclimber at work (a) and example case of Roboclimber on a flat sloping wall (b)
The equilibrium problem under unilateral constraints is solved iteratively. First, all constraints are supposed bilateral and the displacements of the hip and rope points are computed. If, at some hip and rope points, the computed displacements are such that the feet detach from the ground or the ropes become slack, the constraints at these point are released one by one and each time the equilibrium problem is solved again. The solving path branches. The computation stops when the displacements at the points with nonreleased constraints are such that all feet are leaned on the ground and all ropes pulled. In this case, the solution under bilateral constraints is the same as the solution under unilateral constraints. If no solution is found along any branch of the path tree, the configuration is not of equilibrium under unilateral constraints. The following sections present the modeling of robot and ground, and the iterative solving procedure. The method is applied step by step to the CLAWAR Roboclimber, for which it was firstly developed. Roboclimber is briefly described in the next section.
2. Roboclimber Roboclimber is a four-legged, two-roped CLAWAR conceived and designed to automatize heavy duty operations in civil and infrastructures engineering in harsh environment (The roboclimber partnership, 2003; Moronti et al., 2004) (Fig. l(a)). The robot was developed within the homonym CRAFT European Project ended in 2004, starting from a solution conceived at the PMARlab of the University of Genoa following an original idea of the climber-man Roberto Zannini. Roboclimber (Fig. 2(a)) is composed of a mobile platform equipped with four 3-dof semi-cartesian legs and two rope winches. The main actuators are hydraulic, powered by an electrical unit placed on board. The platform carries an operative tool that in the case of the first prototype of Roboclimber is a drilling system with an automatic rod storage. The drilling rods are handled and fetched into the drilling head by a special purpose manipulator that loads and unloads the rods on/from the rod storage and the drilling machine,
Fig. 2 Virtual mockup of Roboclimber showing the subsystems (a) and an example of intervention map (b) with the expected hole locations; the grey working area is the zone of the wall that Roboclimber can reach with the two assigned anchorage points
Springer
202
Auton Robot (2006) 21:199–210
Fig. 3 Generic CLAWAR with legs and ropes (a); modeling of the leg, rope-robot body and terrain-leg compliances (b); and choice of the three dummy points and rigid-body constraint (c)
so that the onboard system is able to make deep holes by screwing many rods and then to recover the rods completely automatically. Electrical power and signals connections are supplied through an umbilical joining the robot to the ground station placed in a safe area at the bottom of the wall. The operator can tele-control Roboclimber from the remote station placed in a convenient site, communicating via radio with the ground station. Onboard CCD cameras and geophysical sensors provide information about the progress of the work and the characteristics of the rocks. The main application to which Roboclimber is addressed is the consolidation of mountain faces and rocky walls subject to landslide and fall of material. To stabilize the rocks and make safe the site, the different layers of rock and eventually big stones included in the layers are packed and pressed each other by means of long piles (up to 20–30 m in depth and 120 mm in diameter) biting deeper stable rocks. The setup of these piles is a though task: deep holes with large diameters have to be created at tens/hundreds of meters in height from the base of the wall and the machinery for that is very heavy. In the past and for small interventions these drilling operations were done manually by squads of climbing workers; usually, for larger interventions, scaffolds are mounted in front of the rocky wall or telescopic cranes are used (this only for small interventions on walls whose bases can be reached by conventional wheeled vehicles). Roboclimber represents a definitive alternative to these traditional intervention techniques: it can walk down the truck used for the transportation and reach the base of the rock face to be consolidated; then its ropes are fixed to anchorage points prepared at the top of the rock face and the robot starts climbing. The locations of the holes to be performed are marked in a map of the intervention prepared during a previous geological survey of the site (Fig. 2(b)). The tele-operator pilots the robot to the different wall locations where holes are planned and then the system goes on automatically with the holing procedure. Roboclimber can walk without ropes on surfaces up to 30 degrees sloping; with ropes, it can climb walls up to almost vertical. The maximum obstacle that it can overcome is a
Springer
cube of 0.5 m side. The overall mass including the drilling system is about 3500 kg. The mass of the legged-roped mobile platform (without drilling system) is about 2000 kg and its payload is 1500 kg.
3. Modeling of the system “robot-terrain” Consider a generic CLAWAR with N r ropes and N f legs (Fig. 3(a)). The configuration is assigned. Each leg is connected to the robot body at a hip point and ends with a foot, that we imagine as a contact point at the ground surface. The foot point of the i-th leg is named Bi , i = 1, . . ., N f ; ni , is the outward-pointing normal vector on the ground surface at Bi . The hip point of the i-th leg is Pi . The ropes are connected to the robot at the points Pi , i = (N f + 1), . . . , (N f + N r ). The direction of the i-th rope is represented by the unit-length vector ni , from Pi toward the ground point where the rope is anchored. The length of the rope is li . We introduce three more dummy points PN f +N r +1 , PN f +N r +2 , PN f +N r +3 , fixed to the robot body. The total number of robot body points considered is N = N f + N r + 3. The external forces and moments applied to the robot are replaced by an arbitrary, statically equivalent system of forces and moments composed of forces fi applied at the points Pi and moments mi , i = 1, . . . , N. Of course, some fi and mi may be zero. The robot is modeled as an elastic system composed of a rigid body (the robot body) connected to the environment by compliant ropes and supported by compliant legs whose feet lean on a compliant terrain. Without external forces and moments applied to the robot (initial configuration), the location of Pi in an arbitrary Oijk ground reference frame is represented by a vector named xi . Once the fi and mi are applied, the robot body moves on its elastic supports (the ropes and the legs with the feet on a compliant terrain) (deformed configuration), and the new locations of the points Pi are
Auton Robot (2006) 21:199–210
203
xi + ui . The vectors ui represent the displacements. If the ground point coincident with Pi , in the initial configuration, −−→ is P¯i , i = 1, . . . , N, then ui = P¯i Pi . Due to the compliance of the terrain, also the foot points Bi move when passing from the initial configuration to the deformed configuration. Their displacements are represented by the vectors uiB . The entries of the ui are gathered in the 3N-entries vector u: u = u1T
...
T T
uN
(1)
The reaction force at point Pi in the deformed configuration is ri . We need a model for the force-displacement behavior of the terrain, one for the ropes and one for a generic robot leg. We also need a computationally efficient formalization of the rigid-body constraint, involving all points Pi . After, we can write the equilibrium equations for the robot body and we can solve the static equilibrium problem. In the paper, if not differently specified, the vectors are always expressed in the ground reference frame Oijk. Roboclimber—In the following we discuss step by step the application of the method to Roboclimber. The foot points are P1 , P2 , P3 , P4 , numbered counter-clock wise from the back-right foot; the ropes are connected to the robot body at the points P5 and P6 (Fig. l(b)). As an example, we consider a flat terrain represented by a wall plane forming an angle α with the horizontal. The ground normal vector ni at the four feet is the same ni = n = [ − sin α 0 cos α ], i = 1,. . ., 4. The center of mass of the robot body is supposed at a distance zG from the wall plane which the robot moves on. The anchorage points are disposed at the same height at an horizontal distance c = 4 m each other. The position of Roboclimber is represented by the distance h of its center of mass (measured parallel to the plane) from the line connecting the anchorage points, and by the lateral shift t of its center of mass from a vertical plane orthogonal to the wall plane through the midpoint of the line connecting the two anchorage points. The ropes have a significant influence on the stability of Roboclimber; in particular, the tension forces they apply can result in lateral overturning of the robot. The locations of the anchorage points must be attentively selected in order to make reachable the larger zone of rocky wall with the minimum number of repositioning operations.
thus the directions ni , i = (N f + 1),. . ., (N f + N r ), are supposed independent from the ui . The i-th frame-rope compliance depends on three contributions:
r the compliance of the rope, r the compliance of the device operating the rope (e.g., a winch) and
r the compliance of the robot body in the surroundings of the rope point Pi . Further, the robot body is treated as a rigid body. These three compliances are modelled like three linear springs in series in direction ni , with stiffness constants, respectively, fr kirr (per unit length of rope), kiwr (for the winch) and ki for fr the robot body. The value of ki can be mapped (e.g., by FEM analysis of the robot model) for a set of directions ni of the rope. The combined stiffness constant of the three springs is kir = ( li k1rr + k1wr + 1f r )−1 and the reaction force at the i-th ki i i rope point Pi is: ri =
−Ki ui
ui · ni ≤ 0
03
ui · ni > 0
(2)
where the stiffness matrices are Ki = kir ni niT , i = (N f + 1) ,. . . , (N f + N r ), and 03 is the 3-entries zero vector. The constraint provided by the rope is unilateral and the reaction force is null if ui · ni > 0. We can replace each rope with a compliant support connecting P¯i , and Pi , and providing a reaction force satisfying Eq. (2). The compliance of this support is described by the stiffness matrix Ki , (compare Fig. 3(a) and (b)). Roboclimber—In the case of Roboclimber, the ropes are twisted-strand steel wires 16 mm in diameter connected to the frontal end of the frame cage and operated by special r (http:/www.tractel.com ), winches named Tirfor Fig. 2(a). The stiffness constant per unit length for these wires is about k5rr = k6rr = 3.9 107 mN2 . The Tirfor winch pulls the rope by means of a system of cams that can be considered rigid, thus the contribution of the Tirfor to the overall rope-robot compliance is supposed negligible. The fr ki are mapped by local FEM analysis of the frame cage (Fig. 4). Due to the geometry of the cage close to the rope fr points, ki is almost independent from ni . The average value fw fw N that has been computed is k5 = k6 = 4.3 108 m , which is one order higher than the rope stiffness constant.
3.1. Rope-robot body compliance 3.2. Terrain compliance The ropes are modelled as flexible, linear, elastic tension-only cables. The displacement ui of the rope point Pi is supposed small compared to the length li of the rope,
Terrains have very complex force-displacement behaviors. Here, we accept a strong approximation. The ground reaction
Springer
204
Auton Robot (2006) 21:199–210
Fig. 4 Local FEM analysis of the frame cage to map k f wi
force is supposed linear in the displacement and orthotropic with respect to the direction ni . In any local ground-surface reference frame centered at B¯ i , i = 1, . . . , N f , with the first axis in direction ni , the terrain stiffness tensor is described by a 3 × 3 diagonal g gn gp gp gn gp matrix Ti = diag(ki , ki , ki ), where ki and ki are the orthotropic stiffness constants of the terrain, respectively, in direction ni and in the plane orthogonal to ni . The reaction force due to a displacement uiB of the foot point Bi is: riB =
gT
g
g
−Ri Ti Ri uiB
uiB · ni ≤ 0
03
uiB · ni > 0
−1 gT g−1 g Ki = Li−1 + Ri Ti Ri
(3)
g
3.3. Combined leg-terrain compliance Each leg is a compliant system connecting the foot point Bi , i = 1, . . . , N f , to the hip point Pi We suppose that for small displacements the force that the leg applies between Bi and Pi is proportional to the relative displacement of Bi and Pi and we replace the leg with
r a linear compliant support connecting points P¯i and Pi , and
r an external moment md due to the non-coincidence of i points Pi and Bi (compare Fig. 3(a) and (b)). The foot leans on a compliant terrain and we take into account this compliance as a part of the compliance of the support replacing the leg. Leg and terrain compliance are in series. We introduce a combined stiffness tensor (leg and terrain) that in Oijk is described by the matrix Ki , i = 1, . . . , N f . The value of the external mo−−→ ment mid is mid = Pi Bi × ri = Di ri , where Di , is the skew −−→ matrix of Pi Bi .
(4)
The reaction force ri , at Bi (and thus at Pi ) is: ri =
−Ki ui 03 g−1
where Ri is the rotation matrix from Oijk to the local ground-surface reference frame. If uiB · ni > 0, the i-th foot leaves the ground and the constraint reaction is null. Roboclimber—Roboclimber is supposed to move on a rock surface, which is rigid compared to the compliance of the legs, so we do not consider any terrain compliance.
Springer
First, consider one generic leg. Since by hypothesis the reaction at the foot is a pure force without moment component, the stiffness tensor representing the compliance of the leg is described by a 3 × 3 stiffness matrix. In the ground reference frame Oijk, we name this matrix Li . The reaction force at Bi due to a displacement (ui − uiB ) is ri = −Li (ui − uiB ). Matrix Li , can be mapped off-line for a suitably dense set of leg configurations. If the architecture of the leg is simple, Li can be also expressed in analytical form, as shown in the following for Roboclimber. The combined stiffness leg-terrain is described by Eq. (5). The stiffness tensor of the combined compliance leg-terrain is described in Oijk by the 3 × 3 matrix:
g−1
Ki ui ≤ T g−1 ni Ti Ki ui > 0 niT Ti
0
(5)
where Ti Ki ui = uiB . The external moment mid is a function of the displacement and depends on Ki , as well: mid = −Di Ki ui . (A threshold on the value of the component of ri , tangential to the terrain, can be eventually introduced in the case that slippery of the foot is expected for forces above that threshold). Roboclimber—The legs of Roboclimber (Fig. 5) have a semi-cartesian RPP (Revolute-Prismatic-Prismatic) architecture with the two prismatic joints orthogonal each other, with the second prismatic joint parallel to the axis of the hip revolute joint (R). From the design point of view, the leg is composed of a hip link hinged to the frame cage. The thigh link slides inside the hip link, while the calf link slides in a square tube welded at one extremity of the thigh. Both thigh and calf have the same square hollow cross-section for modularity purpose. The foot is a steel hollow cone mounted at the extremity of the calf link. The actuators are hydraulic cylinders: two identical cylinders hosted inside the thigh and the calf, for the translations and one external cylinder connecting the frame cage to the hip link for the hip rotation. Friction is reduced by means of suitable plastic sliding panels disposed inside the prismatic pairs. An approximate stiffness matrix for the leg can be easily written in a leg reference frame with one axis parallel to the thigh and one parallel to the calf. Consider Fig. 5(b). We refer to a simplified model of the leg composed of two beams of lengths lit and lic , with the same moment of inertia J (the same for both beams in all four legs), welded orthogonally (the knee is considered ideally rigid, without any rotational compliance). The axial stiffness of these beams is supposed
Auton Robot (2006) 21:199–210
205
Fig. 5 Prototype of Roboclimber: detailed view of the leg (a) and reference parameters (b)
equal to the axial stiffness kj of the hydraulic cylinders used for the actuation. Note that the force-displacement relation for an hydraulic cylinder commanded by servo valves is quite complex (Richard and Vivalda, 2002); here we accept a strong simplification and suppose that the force is proportional to the displacement for small displacements. The hydraulic cylinder for the hip rotation is connected to the thigh at a distance lia from the hip axis. It forms an angle φia with the direction of the thigh P-joint and its axial stiffness is kj as for the other cylinders. The parameters lit , lic and φia define the configuration of the leg. The leg stiffness matrix in Oijk is obtained from the stiffness matrix in the leg reference frame, by means of the rotation matrix Rli from Oijk to the leg reference frame: T Li = 6E J k j Rli
2 2 k j lic3 +3E J +3k j lit lic #2
0 3k j lic lit #2
2
0 G sin2 φia #3
0
3lic k j lit #2
0
2
2#1 #2
2 #2 = 27E Jlit + 3#1lit + 4lic #1 k j lic +
The reaction forces ri , associated to the displacements ui in the deformed configuration satisfy the force and moment equilibrium equations of the robot body. We write these equations under the hypothesis that all constraints are bilateral. Thus, in Eqs. (2) and (5), we consider the force-displacement relations holding for ui · ni ≤ 0. The presence of unilateral constraints will be taken into account later. The six equilibrium equations are: (Ki ui − fi ) = 03
(7)
i=1
E J #1 N
3 2 #3 = 2Glic k j + 3Elit lic k j + 2G#1 sin2 φia +6G E J cos2 φia
3.4. Equilibrium Equations
N
3
#1 = 3E J + lit k j la 12 it li
Since the terrain is supposed rigid (rock), the compliance Ki of the support replacing the leg is the same as the leg compliance: Ki = Li . (Due to the shape of the feet (sharp cones scratching the rock), no foot-rock slippery is considered).
i=1
(6)
In Eq. (6), Li is expressed as a function of the leg configuration through lit , lic and φia . The values of J and kj can be selected by a preliminary FEM analysis of the leg (to adapt the model to the real leg) and no mapping of the entries of Li on the leg workspace is required.
N f
((xi + ui ) × (Ki ui − fi ) − mi ) −
Di Ki ui = 03 (8)
i=1
Note that the operations of sum in Eqs. (7) and (8) are extended also to the three dummy points, for which we use as stiffness matrices (Ki , i = N − 2, N − 1, N ) three zeromatrices. This is a bit computationally inefficient but the advantage is that we can more easily collect together the equilibrium and the rigid body equations, as shown in the following.
Springer
206
Auton Robot (2006) 21:199–210
Equation (7) are linear combinations of the displacement components of u, while Eq. (8) contain also second-degree terms in the components of u. First we gather the constant terms of the six equations, by rows, in the 6-entries vector bP : b = P
N
fT i=1 i
N i=1
(mi + fi xi )T
T (9)
where, again, the symbol “tilde” over a vector indicates skew operation. The coefficients of the first-degree terms of the six Eqs. (7) and (8) are gathered in the (6 × 3 N) block matrix AP , one row per Equation:
...
K1
AP =
x1 K1 + f1 . . . 033 ...
+
D1 K1
...
KN
x N KN + fN 033 06(N r +3) DN f KN f
(10)
The second-degree terms in Eq. (8) can be expressed as three quadratic forms in the components of u, one for each equation. We gather them in the 3-entries vector qP : q P = uT Q1P u uT Q2P u
T uT Q3P u
(11)
The coefficient matrices Q1P , Q2P , Q3P , are block matrices. The entries on the diagonal are non-null matrices; all entries out of the diagonal are zero matrices: [03 Ki k − Ki j]T i = j (12) Q1iP j = i = j 033 Q2iP j =
Ki i]T
[Ki j
i= j i = j
033
Q3iP j =
[−Ki k 03
− Ki i
03 ]T
033
i= j i = j
(13)
(14)
P P where Qhi j is the (i, j) block of matrix, Qh , h = 1, 2, 3, i; j = 1, . . . , N. The final form for the six equilibrium equations is: (15) qP + AP u = bP
3.5. Rigid-body constraint The distance between whatever couple of points Pi is supposed the same in the initial and deformed configuration. To formalize this constraint, we choose the three dummy points,
Springer
PN f +N r +1 , PN f +N r +2 , PN f +N r +3 such that no other Pi lies in the plane they define. The three dummy points can be different for different robot configurations. Then, we state that every distancePi PN −h+1 , h = 1, 2, 3 i = 1, . . . , N − h
(16)
is the same in the initial and deformed configuration (Fig. 3(a) and (c)). We obtain the (3N − 6) Equations: (xi + ui − x N −h+1 − u N −h+1 )2 − (xi − x N −h+1 )2 = 0 (17) Each Eq. (17) contains a first-degree term and a second-degree term in the components of the displacements. The first-degree terms are linear combinations of the entries of u. We introduce a (3N − 6) × 3N coefficient matrix AR such that the linear term of the n-th Eq. (17) is equal to the n-th entry of the vector AR u. Matrix AR has a block structure, as suggested by the scheme for the values of h and T T T i shown in Relations (16): A R = [A1R A2R A3R ]T . A1R , A2R and A3R are, respectively, (N − 1) × 3N, (N − 2) × 3N and (N − 3) × 3N matrices. The generic AhR , h = 1, 2, 3, is easily described as a (3N − h) × N block-matrix whose entries are 3-entries row vectors. In index notation: −2(xm − xi )T m = 1 R (18) m = N −h+1 Ahim = 2(xm − xi )T T elsewhere 03 where h and i satisfy Relations (16), m = 1, . . . , N, and (xm − xi ,) is a 3-entries vector. The second-degree terms of Eq. (17), one for each equation, are gathered in the (3N − 6)-entries vector qR . Due to the structure of Relations (16), qR is a block-vector T T T composed of three vectors: q R = [q1R q2R q3R ]T , where q1R , q2R q3R have, respectively, N − 1, N − 2 and N − 3 entries. The i-th entry of the generic qhR , h = 1, 2, 3, is qhiR = (ui − u N −h+1 )2 , where h and i satisfy Relations (16). Finally, Eq. (17) can be rewritten in the compact form: AR u − qR = 03N −6 . 4. Static equilibrium under bilateral constraints In order to solve the robot static equilibrium problem under unilateral constraints, first we provide an algorithm solving the problem under bilateral constraints. The system composed of the robot body suspended on the hip and rope compliant supports (Fig. 3(b)), as described in the previous sections, is statically determinate. This system is described by 3N equations: 6 equilibrium equations
Auton Robot (2006) 21:199–210
207
(Eqs. (7) and (8)) and (3N − 6) rigid-body equations (Eq. (17)); the unknowns are the 3N scalar components of u. The 3N equations can be rearranged in the compact matrix T T q R ]T and b = form: q + Au = b where q = [03T q P T T T T [b P 0(3N −6) ]T are 3N-entries vectors and A = [A P A R ]T is a 3N × 3N matrix. Since the displacements ui , are supposed small compared to the dimension of the robot and we want a low running time, we consider the linearized system of equations: Au = b
(19)
The solution of System (19) is an approximate solution to the static equilibrium problem of the generic CLAWAR with legs and ropes, under the hypothesis that all constraints are bilateral. The geometry and configuration of the robot and the local geometry of the terrain are supposed assigned (for example, the robot configuration is proposed by the gait planner). The entries of the leg stiffness matrices Li , and the stiffness fr constants ki can be read from tables (e.g., filled off-line and stored in the control system of the robot), the stiffness matrices for the terrain are computed and matrix A is singled out. Then, System (19) can be solved numerically. 5. Solving method for unilateral constraints The equilibrium equations in System (19) are written under the hypothesis that all constraints are bilateral. We call u i⊥ the component of the displacement at the i-th foot/rope point parallel to ni : ui⊥ = uiB · ni at foot points; ui⊥ = ui · ni at rope points. With unilateral constraints, the reaction force at any point Pi , i = 1, . . ., N − 3, such that ui⊥ > 0, is zero and the corresponding constraint can be imagined as a bilateral constraint that has been released. From a physical point of view, at a foot point with u i⊥ > 0, the foot detaches from the terrain; at a rope point with u i⊥ > 0, the rope becomes slack. If we are able to recognize which feet detach from the terrain and which ropes become slack, then we can compute the solution under unilateral constraints by means of the algorithm for bilateral constraints (e.g., by means of System (19)), provided that the constraints at the foot and rope points corresponding to detached feet and slack ropes have been considered released. For any given robot configuration, we can proceed iteratively: first, we solve the case with bilateral constraints applied at all points Pi and obtain a set u of displacements. Then we release one by one the constraints at the points Pi , with u i⊥ > 0, i = 1, . . . , N − 3. Each time we release a constraint, we recompute the equilibrium for the new system considering the nonreleased constraints bilateral; the solv-
ing path follows the branches of a tree. Figure 6(a) presents an example of a generic solving tree; in each node a table gathers the sign of u i⊥ at the different feet/ropes Pi . The starting configuration is on the right of the picture; it is solved and some positive u i⊥ are found (number 2, 3 and N); one new robot configuration (node) is generated per each positive u i⊥ by eliminating the corresponding foot/rope (black square): in the node coming from number 2, for example, the foot/rope i = 2 is eliminated (black square) and the equilibrium of the new robot (with one foot/rope less) is recomputed. In case of further positive u i⊥ , new leaves are generated. The computation along a branch of the tree stops in two cases:
r If the displacement ui , at any point Pi with an active con-
straint satisfies u i⊥ ≤ 0. In this case, the solution of the equilibrium problem under unilateral and bilateral constraints is the same and the problem is solved. r If all constraints but three have been released and for some of these three unreleased constraints u i⊥ > 0. In this case, no stable static equilibrium under unilateral constraints is possible along this branch of the tree. In fact, with less than three feet/ropes, stable equilibrium is no more possible. The analysis goes on along all branches of the tree until one equilibrium configuration is singled out or until all branches have been analyzed. The same equilibrium configuration can be found along more than one branch, i.e., by releasing the constraints in different sequences. If all the branches of the tree were analyzed and no stable equilibrium configuration was found, we conclude that the starting configuration is not of static equilibrium under unilateral constraints. Roboclimber—System (19) and the iterative solving algorithm for unilateral constraints have been implemented in C + + for any number of legs and ropes, using LAPACK++ routines. The result is a general routine to asses if any multi-roped, multi-legged robot, in any assigned configuration, is or is not in static equilibrium. The general routine has been used to solve the static equilibrium of Roboclimber. The number of legs has been set to four and the number of ropes to two. The tables with the stiffness parameters have been filled and stored as described in the previous sections and a test campaign performed. The average running time measured during the tests on a PC 2.0 GHz PII with 1047 MB RAM has been about 2 ms. Figure 6(b) shows part of a possible solving path for an example configuration of Roboclimber. We have six feet/ropes. First the equilibrium problem is solved considering all feet and ropes (starting configuration on the right). Some u i⊥ are positive. For each of them a new node is generated with the corresponding foot/rope eliminated. In each node, the setup of Roboclimber is synthetically recalled by the small picture Springer
208
Auton Robot (2006) 21:199–210
Fig. 6 Example solving tree for any CLAWAR (a) and for Roboclimber (b)
in the right. The equilibrium configurations (solving the equilibrium problem under unilateral constraints) are in dashed frame. The cross indicates a configuration along a branch at which the procedure stops without finding a solution. The same solution can be singled out along several different branches of the tree. The main use foreseen for this static-equilibrium routine is in the planning algorithm that plans online the gait of Roboclimber. This is described in detail in Cepolina et al. (2006) and Moronti et al. (2004). The goal is singling out a sequence of static equilibrium robot configurations (steps), from a starting location to a given goal location, e.g. defined by the system tele-operator or obtained by the control system from maps of the intervention. An informed search algorithm based on best-first logic is used to progressively generate a tree of feasible configurations of Roboclimber from the initial to the goal location. This tree is progressively expanded and explored with reference to a heuristics using the proposed equilibrium routine to generate a feasible minimum-cost gait. Other off-line uses of the proposed equilibrium routine are prospected, in particular:
r to generate maps of the rocky wall regions where the robot is in static equilibrium;
r to plan the repositioning operations of the anchorage points at the top of the rocky wall; and
r to map the rocky wall regions where drilling operations can be performed without introducing additional devices to anchor the robot to the rocky wall. These data are useful to operate Roboclimber and to prepare the plan of the consolidation operations. Part of the obtained information can be used in the HMI of the robot. We compute three different kinds of stability maps, corresponding to different combinations of the influencing parameters (in the pictures an example flat rock wall is considered;
Springer
the same maps can be generated for whatever wall geometry including very irregular walls (Cepolina et al., 2006)):
r The planes (t, h) at different values of α, with c and zG fixed; these maps represent how the region of static equilibrium changes while changing the sloping angle of the wall (Fig. 7). r The planes (t, z G ) at different values of α and h, with c fixed; these maps represent the influence of zG on the static equilibrium of the robot (Fig. 8). r The planes (t, c), with all other parameters fixed; these maps represent the influence of the distance between the anchorage points. They can be used to reduce the number of repositioning operations of the anchorage points (Fig. 8). These stability maps can be computed starting from a configuration of Roboclimber with 3 or 4 feet leaning on the wall, with or without drilling forces and moments applied to the robot. The maps computed starting with a configuration with any 3 feet on the wall and without drilling forces and moments are used to check which legs can be operated in the different zones of the wall to perform one step (since to perform one step it is necessary to detach one feet from the ground and move it at another wall location). The maps computed considering 4 feet on the wall and in presence of drilling forces and moments are used to show where drilling operations can be performed by Roboclimber without the need of additional devices fixing the robot to the wall. Due to the high values of these forces and moments, the robot can perform the drilling only in a sub region of the reachable wall region. Note that the drilling force, that is almost orthogonal to the wall, is counterbalanced by the component of the gravity force in direction orthogonal to the wall; this component is as lower as higher the sloping α of the wall, therefore on very steep slopes the wall region
Auton Robot (2006) 21:199–210
where drilling can be performed without using additional anchorage devices is very narrow. 6. Conclusions The paper proposes a fast method to compute an approximate solution of the static equilibrium problem of multi-legged, multi-roped walking robots with quasi-static locomotion. On very steep and/or uneven terrains it is necessary to check at any step if the gait performed by the robot is really quasi static (a succession of static configurations). This is complex for multi-legged robots (and much complex in presence of actuated ropes connecting the robot to the environment), since the equilibrium problem is statically-indeterminate. Moreover, the constraints provided by the legs and the ropes are unilateral. The method proposed in the paper considers the elastic characteristics of the robot and the ground to make the problem solvable. The solving procedure is iterative. It is two-folds: an algorithm to solve the equilibrium problem
209
under bilateral constraints is provided; this algorithm is applied iteratively to solve the case with unilateral constraints. The two levels are independent, so it is possible to adapt the algorithm for bilateral constraints to the particular robot architecture considered without changing the iterative solver that solves the system under unilateral constraints (and then without changing the solving software). In the first development of the CLAWAR robot Roboclimber, the proposed algorithm is used to plan the missions of the robot and in the Human Machine Interface to help the tele-operator in manoeuvring the robot correctly and safely. A new European Project, named Saferdrill, started in October 2005. In its frame, the level of autonomy of Roboclimber will be extended also to the phase of movement on the rocky wall. The proposed algorithm will be implemented in the control system and it will be used in the path planner and for continuous online assessment of the degree of equilibrium of the robot in order to improve the overall safety. Afar from instable configurations, small changes in the initial configuration of the robot should result in small
Fig. 7 Examples of planes (t, h) for α = 70◦ (left) and α = 80◦ (right) (a) and physical explanation of these planes (b) Fig. 8 Examples of planes (t, zG ) for h = 10 m and h = 5 m (left); planes (t, c) for zG = 1.8 m and zG = l.2 m (right)
Springer
210
variations in the solution singled out by the equilibrium algorithm (maybe taking into account the whole solving tree instead of stopping at the first solution singled out). A planning algorithm using the proposed equilibrium algorithm should be able to generate a continuous gait (see (Cepolina et al., 2006; Moronti et al., 2004) for further discussion). The solving technique presented can result advantageous also in domains different from CLAWARs, in particular for grasping analysis in multi-fingered grippers. Acknowledgments Part of this research has been developed within the EU CRAFT Project Roboclimber (contract No. G1ST-CT-20025016) funded by the EU Commission. The EU Commission and all the partnership are hereby kindly acknowledged. The EU Commission is gratefully acknowledged also for funding the Cooperative Research Project SAFERDRILL.
References Saranli, U., Buehler, M., and Koditschek, D.E. 2001. Rhex: A simple and highly mobile hexapod robot. Int. J. of Robotics Research, 20(7):616–631. Gonzalez de Santos, P., Galvez, J.A., Estremera, J., and Garcia, E. 2003. Silo4—a true walking robot for the comparative study of walking machine techniques. IEEE Robotics and Automation Magazine, 10(4):23–32. Scholl, K-U., Gafb mann, B., and Berns, K. 2001. Locomotion of LAURON III in rough terrain. In IEEE/ASME Int. Conf. on Advanced Intelligent Mechatronics, Como, Italy, July 8–11. Apostolopoulos, D. and Bares, J. 1995. Locomotion configuration of a robust rappelling robot. In IEEE Int. Conf. on Intelligent Robots and Systems, Human Robot Interaction and Cooperative Robots IROS’95, Vol. 3, pp. 280–284. Krishna, M., Bares, J., and Mutschler, E. 1997. Tethering system design for Dante II. In IEEE Int. Conf. on Robotics and Automation, Vol. 2, pp. 1100–1105. Wettergreen, D., Pangels, H., and Bares, J. 1995. Behavior-based gait execution for the Dante II walking robot. In Int. Conf. on Intelligent Robots and Systems, Pittsburgh, PA, USA, August 5–9, pp. 274– 279. Song, S.M. and Waldron, K.J. 1989. Machines That Walk: the Adaptive Suspension Vehicle. MIT Press. Ting, L.H., Blickhan, R., and Full, R.J. 1994. Dynamic and static stability in hexapodal runners. Experimental Biology, 197(1):251– 269. Hirose, S., Yoneda, K., and Tsukagoshi, H. 1997. TITAN VII: quadruped walking and manipulating robot on a steep slope. In IEEE Int. Conf. on Robotics and Automation, Albuquerque, NM, USA, April 20–25, pp. 494–500. The roboclimber partnership. Roboclimber 2003. In P. Fiorini, (ed.), 1st Int. Workshop on Advances in Service Robotics ASER03, Bardolino, Italy, March 13–15, pp. 57–64.
Springer
Auton Robot (2006) 21:199–210 Molfino, R.M., Armada, M., Cepolina, F., and Zoppi, M. 2005. Roboclimber, the 3 tons spider. Int. Jour. Industrial Robots, 32(2):163– 170. Cepolina, F., Moronti, M., Sanguineti, M., Zoppi, M., and Molfino, R.M. 2006. Roboclimber versus landslides. Int. Jour. IEEE Robotics & Automation Magazine, 13(1):Under the press. Fukushima, E.F., Kitamura, N., and Hirose, S. 2001. Development of tethered autonomous mobile robot systems for fieldworks. Advanced Robotics, 15(4):481–496. Hirose, S. and Fukushima, E.F. 2002. Snakes and strings: New robotic components for rescue operations. In 8th Int. Symp. on Experimental Robotics ISER’02, page CD proceedings, Sant’Angelo d’Ischia, Italy, July 8–11. Dimentberg, F.M. 1965. The screw calculus and its applications in mechanics. Technical Report Technical Report FTD-HT-23-163267, Foreign Technology Division, Wright-Patterson Air Force Base. Ghafoor, A. and Kerr, D.R. 1992. n-grasp robotic fine motion with frictionless elastic point contacts. Proc. Inst. Mech. Eng., Part C: J. Mech. Eng. Sci., 206:41–47. Griffis, M. and Duffy, J. 1993. Global stiffness modelling of a class of simple compliant coupling. Mech. Mach. Theory, 28(2):207– 224. Ciblak, N. and Lipkin, H. 1994. Asymmetric cartesian stiffness for the modelling of compliant robotic systems. In Proc. ASME 23rd Biennial Mech. Conf., Des. Eng., Vol. 72, New York, NY, USA. Ciblak, N. 1998. Analysis of Spatial Stiffness and Applications. Ph.D. dissertation, Georgia Institute of Technology, Georgia, USA. Ciblak, N. and Lipkin, H. 1998. Synthesis of stiffness by springs. September 11–14. Zefran, M., Kumar, V., and Croke, C. 1999. Metrics and connections for rigid body kinematics. Int. J. Robot. Res., 18(2):243–258. Zefran, M. and Kumar, V. 2002. A geometrical approach to the study of the cartesian stiffness matrix. ASME J. of Mechanical Design, 124:30–38. Ghafoor, A., Dai, J.S., and Duffy, J. 2004. Stiffness modelling of the soft-finger contact in robotic grasping. ASME J. of Mechanical Design, 126:646–656. Hiller, M., Schuster, C., and Adamski, D. 1997. Fasim C++—a versatile developing environment for vehicle dynamics simulation. Int. J. of Crashworthiness, 2(1):91–105. Schneider, M. and M¨uller, J. 1999. Force based motion control of the walking machine alduro using exact linearization methods. In IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Atlanta, Georgia, USA, September 19–23, pp. 537– 542. Schneider, M. Miiller, J., and Killer, M. 2000. Modeling, simulation, and model-based control of the walking machine alduro. IEEE/ASME Transactions on Mechatronic, 5(2):142–152. Moronti, M. Sanguineti, M., Zoppi, M., and Molfino, R. 2004. Roboclimber: proposal for online gait planning. In 7th Int. Conf. on Climbing and Walking Robots CLAWAR04, Madrid, Spain, September 22–24. Richard, E. and Vivalda, J.C. 2002. Mathematical analysis of stability and drift behavior of hydraulic cylinders driven by a servovalve. ASME J. of Dynamic Systems, Measurement, and Control, 124:206–213.