¡Descarga snake robot - diseño y modelos y más Apuntes en PDF de Robótica solo en Docsity!
A 3D Motion Planning Framework for Snake Robots
P˚al Liljeb¨ack, Kristin Y. Pettersen, Øyvind Stavdahl, and Jan Tommy Gravdahl
Abstract— This paper presents a motion planning framework for three-dimensional body shape control of snake robots. Whereas conventional motion planning approaches define the body shape of snake robots in terms of their individual joint angles, the proposed framework allows the body shape to be specified in terms of Cartesian coordinates in the environment of the robot. This approach simplifies motion planning since Cartesian coordinates are more intuitively mapped to the overall body shape of the snake robot. The paper demonstrates the applicability of the framework for realizing different types of three-dimensional motion patterns.
I. INTRODUCTION
Snake robots are robotic mechanisms designed to move like biological snakes [1]. The advantage of such mecha- nisms is their long and flexible body, which gives them the potential to move and operate in challenging environments where human presence is undesirable or impossible. Potential applications of these mechanisms include search and rescue operations, inspection and maintenance, and subsea opera- tions. Due to their complex dynamics and unique forms of propulsion, snake robots pose many open research problems. Numerous approaches to motion control of snake robots have been proposed in the literature, and the reader is referred to [2] for a detailed overview. The majority of previous approaches specify explicitly the motion of each individual joint angle of the snake robot. Motion planning on the joint angle level is, however, a particularly challenging task if we are primarily interested in controlling the overall (macroscopic) body shape of the robot, which is generally the case. For this reason, it would simplify the control problem if we could specify the motion in terms of parameters which are more intuitively mapped to the overall body shape. This idea is related to joint space versus task space control of a conventional robot manipulator [3], whose motion is usually specified in the task space of the end effector and then mapped to the motion of each joint. The ’task’ space of a snake robot is, however, defined in terms of the position of all the links (i.e. not only the end effector) since it is the interaction between the links and the physical environment that creates the propulsion forces. Moreover, unlike most robot manipulators, snake robots are underactuated since they have more degrees of freedom than independent control
Affiliation of P˚al Liljeb¨ack is shared between the Dept. of Engineer- ing Cybernetics at the Norwegian University of Science and Technology (NTNU), 7491 Trondheim, Norway, and SINTEF ICT, Dept. of Applied Cybernetics, 7465 Trondheim, Norway. E-mail: [email protected]. Kristin Y. Pettersen is with the Centre for Autonomous Marine Operations and Systems, Dept. of Engineering Cybernetics at NTNU, 7491 Trondheim, Norway. E-mail: [email protected]. Øyvind Stavdahl and Jan Tommy Gravdahl are with the Dept. of Engineering Cybernetics at NTNU, 7491 Trondheim, Norway. E-mail: {Oyvind.Stavdahl, Tommy.Gravdahl}@itk.ntnu.no. This work was partly supported by the Research Council of Norway through project no. 205622 and its Centres of Excellence funding scheme project no. 223254.
inputs. For these reasons, motion planning methods for robot manipulators are not directly applicable to snake robots. Motivated by the above discussion, we propose in this pa- per a general framework which facilitates three-dimensional motion planning for snake robots. Instead of specifying joint angles, the framework allows the motion to be specified in terms of coordinates of shape control points (SCPs). A continuous curve between the SCPs defines the desired shape of the snake robot and is mapped to the individual joint angles through a curve fitting process. The paper demonstrates how the framework can be applied to design complex three-dimensional motion patterns for snake robots. Motion planning for snake robots based on continuous curves has also been considered in previous literature [4]–[7]. This paper is, however, different from these previous works both in terms of the parametrization of the continuous curve and in terms of the approach for generating dynamic motion patterns based on the continuous curve. This paper builds on results presented by the authors in [8], where this motion planning framework was considered in the context of planar (two-dimensional) motion. This paper extends the previous results by showing how the framework can be extended to specify three-dimensional motion. The paper is organized as follows. Section II presents parameters of the snake robots considered in the paper. The motion planning framework is presented in Section III, and its applicability for three-dimensional motion planning is demonstrated by simulation results presented in Section IV. Finally, Section V presents concluding remarks.
II. THE SNAKE ROBOT This section presents the kinematic structure of the class of snake robots considered in this paper. Properties related to the dynamics of the robot (mass, max actuator torque, etc.) are not specified since the proposed motion planning approach is carried out on a purely kinematic level. The general kinematics of the snake robot is illustrated in Fig. 1 using the parameters summarized in Table I. In particular, the snake robot consists of N serially connected rotational joints, where the rotation axes of consecutive joints are orthogonal. The majority of existing snake robots belongs in this class, including snake robots with 2-DOF joint modules where the yaw and pitch axes are intersecting. The kinematic parameters illustrated in Fig. 1 have been defined according to the Denavit-Hartenberg Convention [3], which is a systematic procedure for modelling the kinematics of serially connected joint mechanisms. The specific Denavit- Hartenberg (DH) parameters of the snake robot are listed in Table II. In particular, ai defines the link length between joint i and joint i + 1, di defines the transversal offset of joint i + 1 with respect to joint i (which is assumed to be zero), αi defines the relative angle between the rotation axis of joint i and joint i+1, and θi defines the angle between the
Fig. 1. The kinematics of the snake robot modelled according to the Denavit-Hartenberg Convention. The y axis of each frame is not shown.
Fig. 2. Overview of the motion planning framework.
xi− 1 and xi axis. The angle of joint i is denoted by qi and corresponds to the parameter θi for i ∈ { 1 ,... , N }. Note that the parameter αN − 1 is − π 2 if N is an even number and π 2 if^ N^ is an odd number. The reader is referred to e.g. [3] for a more detailed description of these parameters. The global frame position and orientation of the robot’s head tip is defined by the homogenous transformation matrix
T gh =
[
xh yh zh Oh 0 0 0 1
]
∈ R^4 ×^4 (1)
Furthermore, the global frame position and orientation of frame i ∈ { 0 ,... , N } is calculated as
T gi =
[
xi yi zi Oi 0 0 0 1
]
= T ghT h 0 T 01 (q 1 )T 12 (q 2 ) · · · T i i− 1 (qi)
(2) where
T h 0 =
− 1 0 0 −a 0 0 − 1 0 0 0 0 1 0 0 0 0 1
and where T i i− 1 (qi) is defined for i ∈ { 1 ,... , N } as
T i i− 1 (qi) =
cos qi − sin qi cos αi sin qi sin αi ai cos qi sin qi cos qi cos αi − cos qi sin αi ai sin qi 0 sin αi cos αi 0 0 0 0 1
III. THE MOTION PLANNING FRAMEWORK
In this section, we propose a general approach for speci- fying the three-dimensional motion of snake robots.
A. Motivation and Overview of the Framework
The propulsion of snake robots is generated by body shape changes which induce contact forces from the environment that propel the robot [2]. The most fundamental objective of
TABLE I THE KINEMATIC PARAMETERS OF THE SNAKE ROBOT.
Symbol Description N Number of joints. xg ,yg ,zg ∈ R^3 Coordinate axes of global frame. xh,yh,zh ∈ R^3 Coordinate axes of head frame. xi,yi,zi ∈ R^3 Coordinate axes of frame i ∈ { 0 ,... , N }. The rotation axis of joint i ∈ { 1 ,... , N } is zi− 1. Oh ∈ R^3 Origin of frame attached to the robot’s head tip. Oi ∈ R^3 Origin of frame i ∈ { 0 ,... , N }. a 0 ,aN ∈ R Length of head and tail link, respectively. ai ∈ R Link length between joint i and joint i + 1. qi ∈ R Angle of joint i ∈ { 1 ,... , N }.
a control strategy for a snake robot is therefore to control its body shape. Previous control approaches for snake robots solve this problem by specifying directly the motion of each individual joint. A challenge with this direct joint control approach is that the mapping from individual joint angles to the overall body shape of a snake robot is generally quite complex. For this reason, it would simplify the control prob- lem if we could specify the motion in terms of parameters which are more intuitively mapped to the overall body shape of the snake robot. In particular, a tool which simplifies body shape control will also simplify the task of adapting the motion in challenging and cluttered environments. The motion planning framework proposed in the following is motivated by the above discussion and is summarized in Fig. 2. In particular, the desired shape of the snake robot is specified in terms of shape control points (hereafter denoted SCPs). The SCPs are interconnected by a curve denoted the shape curve, which defines the macroscopic shape of the snake robot. A virtual snake robot (hereafter denoted VSR)
(a) Alignment of the head frame.
(b) Alignment of frame i ∈ { 0 ,... , N }.
Fig. 3. The strategy for aligning the VSR along the shape curve. (a) The VSR is aligned from the head link and backwards. The location sh of the head tip and the roll angle φh of the VSR about the shape curve are inputs to the algorithm. (b) The link from frame i− 1 to frame i is ’aimed’ towards a reference point on the shape curve denoted by sref.
attached to the head tip, frame i ∈ { 0 ,... , N − 1 } attached to each of the N joints, and finally the N frame attached to the tail tip. The output from the alignment algorithm is the origin and orientation of each of these frames. This output includes the angle qi of each joint i ∈ { 1 ,... , N } of the VSR, which the motion planning framework outputs as reference angles for the physical robot.
- The Placement of the Head Frame: The algorithm starts by defining the origin of the head frame as Oh = S(sh). Furthermore, the xh axis is defined as illustrated in Fig. 3(a), i.e. as the unit vector from O 0 to Oh, where O 0 is the origin of the frame of joint 1. The calculation of O 0 is described below, so for now we assume that O 0 has been defined. The xh axis is therefore given as
xh =
Oh − O 0 ‖Oh − O 0 ‖
where we divide by the norm of the vector to make xh a unit vector. As shown in Fig. 3(a), the yh and zh axes are determined by the roll angle φh of the VSR about the shape curve, which is an input to the algorithm. To this end, we first calculate a temporary axis denoted by y
′ h, which we define to be horizontal through the cross product
y
′ h =^
zg × xh ‖zg × xh‖
where zg is the global z axis. A temporary zh axis, denoted by z
′ h, is then calculated as
z
′ h =^ xh^ ×^ y
′ h (7)
The temporary head frame, which is defined by the axes xh, y
′ h and^ z
′ h as shown in Fig. 3(a), is now rotated by the specified angle φh about the xh axis, thereby achieving the specified roll angle of the VSR about the shape curve. The orientation of the head frame with respect to the global frame is thus calculated as
Rgh =
[
xh yh zh
]
[
xh y
′ h z
′ h
]
Rx(φh) (8)
where Rx is the rotation matrix for an elementary rotation about the x axis [3], which is defined as
Rx =
0 cos φh − sin φh 0 sin φh cos φh
- The Placement of the Joint Frames: The origin and orientation of the remaining N + 1 coordinate frames are calculated in consecutive order by following the steps below for each frame. The origin of frame i ∈ { 0 ,... , N } is denoted by Oi and its coordinate axes are denoted by xi, yi and zi, respectively. The algorithm makes use of a reference point sref, which is initialized as sref = sh and then traced backwards along the shape curve to define the desired placement of each frame. To calculate Oi, sref is moved backwards along the shape curve to the point whose linear distance to the previously aligned frame (i.e. Oi− 1 ) equals lLAD ∈ R. The design parameter lLAD is referred to as the look-ahead distance and defines how far backwards along the shape curve to ’aim’ the next link to be aligned. As shown in Fig. 3(b), the location of sref with respect to frame i − 1 is defined as
rref = S(sref) − Oi− 1 (10)
where ‖rref‖ = lLAD. The algorithm attempts to place Oi such that the link between Oi− 1 and Oi (i.e. between joint i and joint i + 1) is aligned with rref. This link is, however, constrained to move in the plane normal to the rotation axis of joint i (i.e. zi− 1 ), which means that a perfect alignment between the link and rref will generally not be possible. The algorithm therefore aligns the link such that the closest possible match with rref is achieved. As illustrated in Fig. 3(b), the closest match is found by first finding a vector r⊥ which is normal to the plane spanned by zi− 1 and rref, i.e.
r⊥^ = zi− 1 × rref (11)
Subsequently, we find the closest match with rref by defining the link between Oi− 1 and Oi to be normal to the plane spanned by r⊥^ and zi− 1. As seen from Fig. 3(b), this is equivalent to defining the xi axis as
xi = r⊥^ × zi− 1 (12)
We are now ready to calculate the angle qi of joint i, which is the angle between the xi− 1 and xi axis (see Section II). We first express the xi axis with respect to frame i − 1. This vector is denoted by xi i− 1 and is found as
xi i− 1 =
Rgi− 1
)T
xi (13)
where Rgi− 1 is the rotation matrix describing the global frame orientation of frame i − 1 , which was determined in the previous step of the algorithm. By denoting the x and
y component of this vector by xi i,x−^1 and xi i,y−^1 , respectively, the joint angle is found as
qi = atan
xi i,y−^1 , xi i,x−^1
where atan2(·) is the four quadrant inverse tangent. Note that xi i,z−^1 is always zero since the xi axis by design is normal to the zi− 1 axis. Using (2), we can now calculate the origin and orientation of frame i with respect to the global frame as
T gi =
[
xi yi zi Oi 0 0 0 1
]
= T gi− 1 T i i− 1 (qi) (15)
where T gi− 1 was determined in the previous step of the algorithm and T i i− 1 (qi) is defined in (4). Remark 3: The choice of look-ahead distance lLAD is important. If we assume that all links of the robot have equal length, i.e. ai = a for i ∈ { 0 ,... , N }, then lLAD should be chosen such that lLAD ≥ a. If lLAD < a, then the alignment will cause each link to ’overshoot’ its reference point on the shape curve. From the authors’ experience, the algorithm will generally perform well if lLAD ∈ [a, 2 a]. Example 4: Fig. 4(b) shows the alignment of a VSR with N = 6 joints along the shape curve plotted in Fig. 4(a). The link lengths were ai = 0. 1 m, where i ∈ { 0 ,... , 6 }, and the look-ahead distance was lLAD = 2a 0.
E. Generating Motion Patterns
General Approach: The presentation so far provides a framework for generating joint reference angles correspond- ing to body shapes defined by SCP coordinates. We can use this framework to generate dynamic motion patterns for a snake robot by varying the SCP coordinates and/or the location of the VSR along the shape curve with time. In particular, motion patterns can be generated in three ways:
Progressing the VSR forward along the shape curve while continuously retrieving its joint angles. The re- trieved angles will vary as the VSR is progressed along the curve, thereby generating dynamic joint reference angles for the physical robot in accordance with the motion pattern drawn out by the shape curve.
Fixing the VSR on the shape curve while varying the SCP coordinates with time. Although the location of the VSR is fixed on the shape curve, its joint angles will vary as the SCP coordinates are changed.
A combination of approaches 1 and 2. With approach 1, the VSR will at some point reach the last SCP of the shape curve. When this happens, we can maintain the motion by extending the curve with new SCPs according to the desired motion pattern. To this end, we introduce two important instruments. The first instrument is the shape frame, which defines the direction in which the shape curve is extended when new SCPs are added. The second instrument is the gait segment, which defines the shape curve over a single cycle of a motion pattern, thereby acting as the ’building block’ of the shape curve. Section IV presents simulation results which illustrate these approaches.
The Shape Frame: When new SCPs are added in order to extend the shape curve, the direction in which the curve is extended will affect the direction in which the snake robot is propelled. We can utilize this property to steer the direction
of the robot’s motion by introducing a coordinate frame whose orientation is defined by a heading controller, and which is used as the reference frame when new SCPs are added to the shape curve. The frame is denoted as the shape frame and, as illustrated in Fig. 5, its axes are denoted by xs, ys and zs, respectively. Moreover, its origin coincides with the last SCP of the shape curve, whose coordinates are P (^) n− 1 = S(n − 1). The orientation of the shape frame with respect to the global frame is expressed by the rotation matrix Rgs =
[
xs ys zs
]
∈ R^3 ×^3 (16)
and can be regarded as a directional control input for the robot.
- The Gait Segment: Snake robots are usually controlled according to predefined gait patterns, which are carried out in combination with feedback control laws that e.g. steer the heading and/or adapt the motion to the environment. To facilitate predefined gait patterns within the motion planning framework, we introduce a gait segment, which describes the shape curve over one cycle of a particular motion pattern. The gait segment is the ’building block’ of the shape curve and is defined as a collection of k SCPs denoted by { P GS 0 , P GS 1 ,... , P GS k− 1
where P GS i ∈ R^3 and superscript ’GS’ is short for gait segment. Sustained motion according to the gait segment is achieved by repeatedly concatenating the shape curve with the gait segment coordinates while the VSR is progressed forward along the curve. The gait segment coordinates are expressed with respect to the shape frame. As illustrated in Fig. 5, an SCP from the gait segment is added to the shape curve by calculating its global frame coordinates P (^) new as
P (^) new = P (^) n− 1 + Rgs
P GS j − P GS j− 1
where P (^) n− 1 is the location of the last SCP of the shape curve, Rgs is the rotation matrix that describes the orientation of the shape frame, and where j ∈ { 1 ,... , k − 1 }. Note that the gait segment is concatenated with the shape curve one SCP at a time since this allows us to steer the direction of the motion by adjusting Rgs each time a new SCP is added. After all k SCPs from the gait segment have been added to the shape curve, the process starts over from the first SCP. Example 5: A gait segment constructed from k = 9 SCPs is plotted in Fig. 4(c) using the same interpolation as in Example 1. The figure also shows the shape curve formed by concatenating the gait segment four times. The shape frame was initially oriented along the axes of the global frame. During the last two concatenations, however, the shape frame was rotated 45 ◦^ about the global z axis by choosing Rgs = Rz (45◦), where Rz describes an elementary rotation about the z axis. As a result, the progression direction of the shape curve was changed by 45 ◦. IV. APPLICATIONS OF THE MOTION PLANNING FRAMEWORK This section presents simulation results which demonstrate the applicability of the motion planning framework for re- alizing different types of three-dimensional motion patterns. The main feature to note from the simulation results is that at no point during the gait design process did we need to consider the specific joint angles of the robot.
Fig. 7. Screen-shots of sidewinding motion simulated in ODE.
The ODE simulation is also visualized in Fig. 7. The results show that sidewinding motion was successfully achieved and that the shape frame rotation successfully steered the robot in the counter-clockwise direction.
C. Simulation 2: Vertical Wave Motion
The next simulation illustrates how propulsion can be achieved by vertical body waves. To this end, we followed the exact same approach as in the previous subsection with the following differences. The parametric curve in (19) was redefined to be a purely vertical wave in the x-z plane by defining Bx(β) = kx 2 βπ , By = 0 and Bz (β) = kz sin(β), where we chose kx = 0. 7 a 0 (N + 1) = 0. 952 m and kz = 0. 1 a 0 (N + 1) = 0. 136 m. We also increased the speed
of the VSR along the shape curve such that
∣ S˙(sh)
∣ = 2^ m/s, and we defined the shape frame to be aligned with the global frame (Rgs = I 3 × 3 ). The simulation result is shown in Fig. 8, where Fig. 8(a) shows the resulting gait segment and the aligned VSR, while Fig. 8(b) shows screen-shots from the ODE simulation which illustrate how the vertical body waves propelled the robot.
(a) The gait segment.
(b) Screen-shots from ODE.
Fig. 8. Simulation results of vertical wave motion. (a) The gait segment and the aligned VSR, where the transversal axis at each joint indicates the rotation axis. (b) Screen-shots of the motion simulated in ODE.
D. Simulation 3: Lateral Rolling Motion During lateral rolling motion, a snake robot moves side- ways by curving its body slightly while rolling about its longitudinal axis. Instead of progressing the VSR forward along the shape curve by continuously increasing sh, we can achieve this rolling motion by keeping sh fixed while continuously increasing the roll angle φh about the curve. In particular, we defined a constant shape curve consisting
(a) The gait segment. (b) The shape curve. (c) The position of the snake robot.
Fig. 6. Simulation results of sidewinding motion. (a) The gait segment and the aligned VSR, where the transversal axis at each joint indicates the rotation axis. (b) The shape curve developed during the motion, where the aligned VSR is plotted at t = 5 s, 10 s, and 15 s, respectively. (c) The position of the centre link (link 9) of the snake robot simulated using ODE.
(a) The shape curve.
(b) Screen-shots from ODE.
Fig. 9. Simulation results of lateral rolling motion. (a) The shape curve and the aligned VSR, where the transversal axis at each joint indicates the rotation axis. (b) Screen-shots of the motion simulated in ODE.
of the three SCPs P 0 = (− 0. 5 a 0 (N + 1), 3 a 0 , 0) = (− 0. 68 , 0. 24 , 0), P 1 = (0, 0 , 0), P 2 = (0. 5 a 0 (N + 1), 3 a 0 , 0) = (0. 68 , 0. 24 , 0). Furthermore, we fixed the head of the VSR at the last SCP by defining sh = 2 and we continuously rotated the VSR about the shape curve by defining φ˙h = − 360 ◦/s. The simulation result is shown in Fig. 9, where Fig. 9(a) shows the shape curve and the aligned VSR, while Fig. 9(b) shows screen-shots from the ODE simulation which illustrate the sideways rolling motion.
E. Simulation 4: Descending a Staircase
The last simulation illustrates the explicit nature of the motion planning framework in terms of defining the body shape of the snake robot. The goal was to make the snake robot in ODE crawl down a staircase with steps that were 0.2 m in both depth and height. This was achieved by defining a gait segment corresponding to a single step of the staircase as shown in Fig. 10(a). As a result, the progression of the VSR along the shape curve, which is plotted in Fig. 10(b) at t = 7. 5 s, caused a staircase-like pattern to be propagated along the body of the simulated robot. This pattern enabled the robot to climb stepwise down the staircase as illustrated in Fig. 10(c).
V. CONCLUSIONS This paper has presented a motion planning framework for three-dimensional body shape control of snake robots, where continuous curves defined by shape control points are mapped to dynamic motion patterns for the snake robot. The framework allows the motion to be controlled by parameters which are intuitively mapped to the overall body shape of
(a) The gait segment. (b) The shape curve.
(c) Screen-shots from ODE.
Fig. 10. Simulation results of a snake robot descending a staircase. (a) The gait segment and a few joints of the aligned VSR, where the transversal axis at each joint indicates the rotation axis. (b) The shape curve developed during the motion, where the aligned VSR is plotted at t = 7. 5 s. (c) Screen-shots of the motion simulated in ODE.
the snake robot. The applicability of the framework was demonstrated by simulation results.
REFERENCES [1] S. Hirose, Biologically Inspired Robots: Snake-Like Locomotors and Manipulators. Oxford: Oxford University Press, 1993. [2] P. Liljeb¨ack, K. Y. Pettersen, Ø. Stavdahl, and J. T. Gravdahl, Snake Robots - Modelling, Mechatronics, and Control, ser. Advances in Industrial Control. Springer, 2013. [3] B. Siciliano and O. Khatib, Eds., Springer Handbook of Robotics. Springer, 2008. [4] G. S. Chirikjian, “Design and analysis of some nonanthropomorphic, biologically inspired robots: An overview,” Journal of Robotic Sys- tems, vol. 18, no. 12, pp. 701–713, December 2001. [5] H. Date and Y. Takita, “Control of 3d snake-like locomotive mecha- nism based on continuum modeling,” in Proc. ASME 2005 Interna- tional Design Engineering Technical Conferences, 2005, pp. 1351–
[6] H. Yamada and S. Hirose, “Study on the 3d shape of active cord mechanism,” in Proc. IEEE Int. Conf. Robotics and Automation, 2006, pp. 2890–2895. [7] R. Hatton and H. Choset, “Generating gaits for snake robots by an- nealed chain fitting and keyframe wave extraction,” in Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 2009, pp. 840–845. [8] P. Liljeb¨ack, K. Y. Pettersen, Ø. Stavdahl, and J. T. Gravdahl, “Com- pliant control of the body shape of snake robots,” in Proc. IEEE Int. Conf. Robotics and Automation, 2014, pp. 4548–4555. [9] F. N. Fritsch and R. E. Carlson, “Monotone piecewise cubic inter- polation,” SIAM J. Numerical Analysis, vol. 17, no. 2, pp. 238–246,
[10] R. Smith, “Open Dynamics Engine,” http://www.ode.org, 2014, online.
