Head-Raising of Snake Robots Based on a Predefined Spiral Curve Method

Transcription

1 Artcle Head-Rasng of Snake Robots Based on a Predefned Spral Curve Method Xaobo Zhang 1,2,3, Jnguo Lu 1,2, *, Zhaoje Ju 1,2,4 and Chenguang Yang 5 1 State Key Laboratory of Robotcs, Shenyang Insttute of Automaton, Chnese Academy of Scences, Shenyang 1116, Chna; zhangxaobo@sacn (XZ); zhaojeju@portacuk (ZJ) 2 Insttutes for Robotcs and Intellgent Manufacturng, Chnese Academy of Scences, Shenyang 1116, Chna 3 Unversty of Chnese Academy of Scences, Bejng 149, Chna 4 Intellgent Systems & Bomedcal Robotcs, School of Creatve Technologes, Unversty of Portsmouth, Portsmouth P1 3HE, UK 5 Zenkewcz Centre for Computatonal Engneerng, Swansea Unversty, Swansea SA1 8EN, UK; cyang@eeeorg * Correspondence: lujnguo@sacn; Tel: Receved: 12 September 218; Accepted: 17 October 218; Publshed: 23 October 218 Abstract: A snake robot has to rase ts head to acqure a wde vsual space for plannng complex tasks such as nspectng unknown envronments, trackng a flyng object and actng as a manpulator wth ts rasng part However, only a few researchers currently focus on analyzng the head-rasng moton of snake robots Thus, a predefned spral curve method s proposed for the head-rasng moton of such robots Frst, the expresson of the predefned spral curve s desgned Second, wth the curve and a lne segments model of a snake robot, a shape-fttng algorthm s developed for constranng the robot s macro shape Thrd, the coordnate system of the lne segments model of the robot s establshed Then, phase-shftng and angle-solvng algorthms are developed to obtan the angle sequences of roll, ptch, and yaw durng the head-rasng moton Fnally, the head-rasng moton s smulated usng the angle sequences to valdate the feasblty of ths method Keywords: snake robots; head-rasng; shape-fttng; phase-shftng; spral curve 1 Introducton A snake robot s a moble robot wth a long and slender body and plays a crucal role n search and rescue operatons, nspecton and mantenance [1] Controllng such robots s dffcult, and a revew of ther modelng, mplementaton, and control can be found n [2] Generally, for gven tasks such as searchng a trapped person n dsaster areas, trackng a flyng object and reparng a damaged ppe as a manpulator, the snake robot has to move ts body wth dfferent moton modes, and head-rasng moton s one of them Snake robots common moton modes nclude serpentne [3,4], travelng wave [5,6], concertna [7,8], and sdewndng [9,1] locomoton Addtonally, some researchers sought to fnd other moton modes such as fuson gat [11] and obstacle-aded locomoton [12] However, these moton modes provde lmted vsual nformaton Consequently, snake robots have to rase ther heads to observe ther surroundngs and perform ther tasks Therefore, the moton plannng for head-rasng of a snake robot s our research object n ths paper To the best of our knowledge, a few researches on head-rasng moton of snake robots have been done, and they can be dvded nto two types: 2D (two dmensonal) head-rasng and 3D (three-dmensonal) head-rasng Ths study ams at 3D head-rasng The concepts of 2D and 3D head-rasng are llustrated n Fgure 1 It should be noted that label 2 and 4 are lne segments models Appl Sc 218, 8, 211; do:1339/app wwwmdpcom/journal/applsc

2 Appl Sc 218, 8, of 2 of snake robots, whch wll be utlzed n subsequent chapters for smulatons 2D head-rasng can be found n [13], the authors proposed an mprovng serpentne nput functon to acheve the head-rasng moton of a snake robot and analyzed the zero-moment pont to assess the stablty of the robot As for 3D head-rasng [14,15], to realze t s more complcated In [14], the works were based on an assumpton that the head part of the robot s not contact wth the ground and regarded as a manpulator, whch s able to acheve several sub-tasks n 3D envronment Therefore, essentally speakng, the researchers manly consdered trackng the trajectory of a snake robot s head rather than the head-rasng process Inspred by the work made by Tanka et al, we currently are workng on trajectory trackng of the snake robot head by mprovng our model There stll exsts problems to be solved and a vdeo about the trajectory trackng (Concept_of_trajectory_trackngav) can be seen n Supplementary Materals In [15], the authors regarded a snake robot as a serally lnked floatng-base robot and dvded t nto base and actve modules The base modules were used to construct the support polygon for mantanng stablty durng the head-rasng process and can be changed to actve modules to allow the head to stretch to a gven poston and orentaton The trajectory functons of ndvdual jonts were set as parameterzed cubc splne curves, and the head-rasng moton was acheved by optmzng these parameters accordng to some crterons In summary, exstng works of head-rasng attempt to fnd the nput functons of ndvdual jonts However, n ths study, the nput functons (angle sequences wth respect to tme) are unknown, and they are obtaned by shape-fttng and phase-shftng algorthms, whch wll be ntroduced n the subsequent chapters 1 2 Target Lne segments model 3 4 Snake Fgure 1 The concepts of 2D and 3D head-rasng Label 1 and 2 represent 2D head-rasng, and label 3 and 4 show 3D head-rasng Achevng moton control for a snake robot s dffcult because of the knematc redundancy, whch can be solved by the shape control method Moton control based on a spral curve belongs to the category of the shape control It s necessary to ntroduce the concept of shape control method Ths method can not be descrbed n theory, and t s a knd of dea to control the moton of snake robots or hyper-redundant manpulators The dea of shape control method s employng a geometrc curve to constran a robot s confguraton by fttng the contnuous curve and dscrete lnk model as closely as possble If change the shape of the geometrc curve wth respect to tme, the robot can acheve contnuous moton The key ponts to use ths method nclude two aspects Frst, how to fnd a proper curve to descrbe the shape of robots should be consdered Second, f the curve s obtaned, there should be an approach to accomplsh the fttng process Examples of usng shape control method can be found n [16 25] In [16], the authors presented a geometrc splne method for the shape control of planar manpulators wth hyper-degrees of freedom Wth the fttng of the robotc

3 Appl Sc 218, 8, of 2 macro confguraton and the referent splne curve, real-tme control could be easly accomplshed In [17], the authors presented locomoton control usng a phase oscllator network, whch can produce a smooth transton of the body shape of a snake robot to avod obstacles In [18], the researchers ntroduced a framework to control the shape of robots, and the framework can be dvded nto four steps, namely, defnng the shape control pont, usng the shape control pont to defne the Bezer curves, fttng the curves and the shape of the robot, and changng the parameters of the Bezer curves to control the movement of the snake robot accordng to the desred gat In [19], the authors used the shape control pont method to control the poston and orentaton of the head of a robot In [2], the researchers presented a planar manpulator that moves accordng to a varable-structure polygon functon produced by neural networks In [21], the authors ntroduced and used a modal approach to generate a backbone curve and thus capture a hyper-redundant manpulator s macroscopc geometrc feature The backbone curve was represented by the lnear combnaton of a set of shape functons wth the approprate coeffcents Then, nverse knematcs reduced to the soluton of these coeffcents to avod the heavy computaton requred by the Jacoban pseudo-nverse method In [22], the researchers analyzed the curvature of the sdewndng locomoton of a snake robot and developed algorthms to ft the robot s shape to a contnuous curve In [23], the authors proposed the annealed chan fttng algorthm and keyframe wave extracton algorthm; the former maps the shape of the snake robot to a contnuous backbone curve and obtans a set of jont angles, whereas the latter obtans a sequence of backbone curves A smlar modelng strategy can also be found n [24,25] In [25], the researchers consdered dynamcs durng the shape control of hyper-redundant manpulators The goal of ths paper s to propose a new approach to rase a snake robot s head n 3D envronments Ths mechansm allows the robot to obtan a 36 feld of vew and a large workng space In ths paper, nspred by the shape control method, a head-rasng moton control method based on a predefned spral curve s proposed The robot s represented by several lne segments, and the correspondng local coordnate system s establshed Every lne segment corresponds to a module of the snake robot, and the connecton pont between adjacent lne segments corresponds to a jont nvolvng three degrees of freedom, namely, roll, ptch, and yaw angles, whch can be obtaned by matrx transformaton of the local coordnate system The predefned curve s ntroduced to constran the macro shape of the snake robot, and a phase-shftng algorthm s developed to drve the snake robot along the curve Durng the phase-shftng process, the angle trajectores for the head-rasng moton n a gven tme can be obtaned Wth the angle trajectores, the head-rasng moton can be acheved The contrbutons of ths work are summarzed by followng remarks: 1 A new shape control curve, the predefned spral curve, s proposed and t s utlzed for 3D head-rasng of a snake robot; 2 A shape-fttng algorthm s developed for adherng the lne segments model of the snake robot to the predefned spral curve; 3 Establshment rules of coordnate system are gven for lne segments model of the snake robot; 4 A phase-shftng and an angle-solvng algorthms are presented for obtanng angle trajectores used durng head-rasng moton The remander of ths paper s organzed as follows Secton 1 revews prevous researches, analyses the necessty for head-rasng and ntroduces our am, whch s rasng a snake robot s head based on a predefned spral curve n 3D envronments Secton 2 ntroduces the modelng of the head-rasng moton; the mathematcal expresson of the predefned spral curve s dscussed n Subsecton 21, the shape-fttng and phase-shftng methods n Subsecton 22, and the establshment of the coordnate system and angle-solvng algorthm n Subsecton 23 Secton 3 explans the smulaton of the head-rasng moton Secton 4 presents the conclusons 2 Modelng of Head-Rasng Moton In ths secton, a predefned spral curve and a shape-fttng method are ntroduced to constran the macro shape of the snake robot, whch s composed of seral lnks represented by seral lne

4 Appl Sc 218, 8, of 2 segments However, the seral lne segments do not contan the complete nformaton of the snake robot Therefore, coordnate system s ntroduced The snake robot has to move through the predefned spral curve from ntal posture to fnal posture, and that process s called phase-shftng Wth the proposed phase-shftng and angle-solvng algorthms, the angle sequences (or angle trajectores) can be used to accomplsh the head-rasng moton 21 Predefned Spral Curve From the bologcal perspectve, a snake s head-rasng movement s rregular, and t s dffcult to completely descrbe ts shape usng a mathematcal model Based on our observatons, the shape of a snake after rasng ts head s smlar to that of a spral curve, as descrbed n Fgure 2 In addton, the spral curve can ensure the stablty of head-rasng Thus, descrbng the head-rasng movement of a snake robot wth a spral curve s reasonable x s (t) = at sn(t + ϕ ) y s (t) = at cos(t + ϕ ) z s (t) = ct (1) Fgure 2 The smlar macro shapes of a snake (label 1), a spral curve (label 2), the lne segments model after the shape-fttng algorthm (label 3) and the smplfed smulaton model (label 4) A regular spral curve (Fgure 3a) can be expressed as Equaton (1) It cannot be utlzed to gude the movement of the robot, and has to be modfed, because t cannot ensure the stablty of head-rasng Then, the predefned spral curve (Fgure 3b) s desgned and ts mathematcal descrpton satsfes Equaton (2), where x l (t), y l (t), and z l (t) represent the lne part; x s (t), y s (t), and z s (t) descrbe the spral curve part; and a, b, and c are the adjustment coeffcents of the spral curve The ampltudes of x s (t) and y s (t) change equally when a s changed and unequally when b s changed The ampltude of z s (t) s nfluenced by c ϕ s the ntal phase of the sne and cosne functons, whch correspond to the slope of the tangent vector for the ntal pont n c s the cycle number of the spral curve l lnk and n are the length and number of the snake robot modules, respectvely t s the ndependent varable dvded nto three ntervals, whch correspond to the lne part (defned by LP, nterval s (, ˆt 1 )); the base of the

5 Appl Sc 218, 8, of 2 spral curve part, whch s n contact wth the ground (defned by BS, nterval s (ˆt 1, ˆt 2 )); and the rest of the spral curve (defned by RS, nterval s (ˆt 2, ˆt 3 )) ϕ base s the phase value of the second nterval x l (t) = x s (ˆt 1 ) t (, ˆt 1 ) y l (t) = y s (ˆt 1 ) t + ˆt 1 t (, ˆt 1 ) z l (t) = t (, ˆt 1 ) x s (t) = ab(ˆt 3 t) sn(t + ϕ ) t (ˆt 1, ˆt 2 ) y s (t) = a(ˆt 3 t) cos (t + ϕ ) 1 t (ˆt 1, t 2 ) z s (t) = t (ˆt 1, ˆt 2 ) x s (t) = ab(ˆt 3 t) sn(t + ϕ ) t (ˆt 2, ˆt 3 ) y s (t) = a(ˆt 3 t) cos(t + ϕ ) t (ˆt 2, ˆt 3 ) z s (t) = ct cˆt 2 t (ˆt 2, ˆt 3 ) ˆt 1 = nl lnk ˆt 2 = ˆt 1 + ϕ base ˆt 3 = ˆt 1 + 2n c π (2) The predefned spral curve s composed of a lne part and a spral curve part, as shown n Fgure 3b The lne and spral curve parts constran the ntal and fnal head-rasng postures of the snake robot, respectvely As shown n Fgure 3, compared wth the regular spral curve expressed n Equaton (1), the predefned spral curve expressed n Equaton (2) ntroduces parameter b to ensure that x s (t) and y s (t) change unequally, nterval (, ˆt 1 ) to construct LP, (ˆt 1, ˆt 2 ) to construct BS, and (ˆt 2, ˆt 3 ) to construct RS Addtonally, by substtutng tem (ˆt 3 t) for t, t can be ensured that x s (t) and y s (t) decrease as z s (t) ncreases Obvously, the predefned spral curve s approprate for ensurng stablty durng head-rasng (a) Lne part (LP) t= Spral curve part (BS) ˆ 2 t= t (b) -2 t tˆ = 3 Spral curve part (RS) t= tˆ Fgure 3 Spral curves: (a) the regular spral curve, and (b) the predefned spral curve used n ths study 22 Shape-Fttng and Phase-Shftng Methods In ths secton, a shape-fttng and phase-shftng algorthms are developed, and the former ensures that the snake robot adhere to the predefned spral curve and the latter drve the robot move along wth the curve Fgure 4 shows the concepts of these methods In Fgure 4, two lnks of robot are shown; the red one represents the robot posture at tme t and the black one corresponds to the robot posture at tme t +1 The robot moves from posture at tme t to posture at tme t +1, and ths process s called phase-shftng Two lnks contan three ponts; that s, the length between adjacent ponts equals the length of the robot lnks Therefore, the snake lnks are calculated by pont sets and represented by lne segments Ths process s called shape-fttng

6 Appl Sc 218, 8, of 2 25 Spral curve Head at t Phase S -1 Head at t 1 Fgure 4 Concepts of shape-fttng and phase-shftng The detals of how the shape-fttng and phase-shftng algorthms work are descrbed n the followng steps Step 1: Intalzaton The parameters a, b, c, n c, n, l lnk, ˆt 1, ˆt 2, ˆt 3, ϕ, and ϕ base are ntalzed The followng constrant equaton s used to ensure that lne segments model of the robot do not get away from the ground S 1 + S 2 = t=ˆt 2 t=ˆt 1 t=ˆt 3 ẋ s (t) 2 + ẏ s (t) 2 + ż s (t) 2 dt+ ẋ s (t) 2 + ẏ s (t) 2 + ż s (t) 2 dt < nl lnk (3) where S 1 and S 2 represent the arc lengths of BS and RS, respectvely t=ˆt 2 Step 2: Determnaton of basc posture The snake robot s represented by lne segments and the correspondng coordnate system Ths secton descrbes how the lne segments of the basc posture are determned The succeedng porton dscusses the rules of buldng the coordnate system The pont sets used to construct the lne segments are defned as follows: P snk = [ P 1, P 2,, P j,, P n+1 ] j = 1, 2,, n + 1 = 1, 2,, n step P j = ( x snk_j, y snk_j, z snk_j ) (4) where P j s the poston of pont j at step of phase-shftng n step s the number of phases shfted and satsfes Equaton (5) =n step S = S 1 + S 2 (5) =1 where S s the arc length at step of phase-shftng and defned as phase n ths study Therefore, shape-fttng reduces to fnd the pont set P snk at step, and phase-shftng reduces to move all lne segments by changng the poston of P snk along wth the predefned spral curve

7 Appl Sc 218, 8, of 2 The basc posture s consdered the ntal confguraton before the phase-shftng begns, as shown n Fgure 5 and calculated as follows P + Tal n 1 Lnk l j P j + 1 j y j z j x j P zj j xj yj Pj Lnk j 1 Local coordnate Reference -2 coordnate z y o o x O o Head P1 Fgure 5 Modelng of the robot s basc posture composed of n lne segments and correspondng coordnate system We set = 1 to ndcate that the lne segments are n the basc posture wthout phase-shftng such that the pont sets of the basc posture satsfy Equaton (6) 1 P snk = (x s (ˆt 1 ) y s (ˆt 1 ) + nl lnk ) (x s (ˆt 1 ) y s (ˆt 1 ) + (n )l lnk ) (x s (ˆt 1 ) y s (ˆt 1 ) + (n j + 1)l lnk ) (x s (ˆt 1 ) y s (ˆt 1 ) ) T (6) Step 3: One step of phase-shftng For programmng purposes, ndependent varable t s regarded as a dscrete vector t d and ts mathematcal descrpton s expressed as follows: t d = [, t 1, 2 t 1,, ˆt 1, ˆt 1 + t 2, ˆt t 2,, ˆt 3 ] (7) where t 1 and t 2 are the step lengths of dscretzaton n ntervals (, ˆt 1 ) and (ˆt 1, ˆt 3 ), respectvely After dscretzaton of t, the predefned spral curve becomes spatal dscrete ponts

8 Appl Sc 218, 8, of 2 Phase-shftng s acheved by the movement of the snake s head ( P 1 ) and the refreshng of the postons of the rest of the ponts ( P 2, P 3,, P j, P n+1 ) along wth the predefned spral curve Frst, gven the phase S, head P 1 changes nto a new poston +1 P 1, durng whch ndependent varable t s assumed to shft from t to t +1 t and S are known, and t +1 can be obtaned usng Equaton (8) t=t +1 S = ẋ(t) 2 + ẏ(t) 2 + ż(t) 2 dt (8) t=t Second, we query t d to dentfy the element nearest to t +1 and record ts ndex as Inx p1 Fnally, the rest of the ponts ( P 2, P 3,, P j, P n+1 ) and correspondng ndces ( Inx p2, Inx p3,, Inx pj,, Inx pn+1 ) can be obtaned by dstance judgment gong through t d from ndex Inx p1 to ndex 1 Dstance judgment s llustrated n Equaton (9) l lnk eps P j P j 1 llnk + eps (9) where eps s utlzed to deal wth the error between contnuous functon and dscrete approxmaton Step 4: Iteraton Step 3 s one step of phase-shftng from step to + 1 On the bass of Equaton (5), all of the values of phase S can be computed manually Then, step 3 s repeated untl the head of the snake ( +1 P 1 ) moves to the fnal pont of the dscrete predefned spral curve Therefore, teraton completes the phase-shftng process from basc posture ( 1 P snk ) to fnal posture ( n step P snk ) of the snake robot (represented by lne segments or pont sets wthout consderng the coordnate system n ths part) 23 Coordnate System and Angle-Solvng Algorthm The lne segments cannot contan all the nformaton of the snake robot lnks or modules Therefore, we propose an approach on establshng the coordnate system fxed on lne segments Fgure 5 shows the reference coordnate O xo y o z o, and ts orgn s [,, ], wth x o -axs [1,, ], y o -axs [, 1, ], and z o -axs [,, 1] In determnng the local coordnates, several rules are needed Fgure 6 shows how these rules are establshed and llustrates one lnk at step durng the phase-shftng process The y j -axs s calculated as follows: y j = p j+1 p j p j+1 p j (1) The x j - and z j -axes are on the plane that s vertcal to the lnk vector shown n the left part of Fgure 6 Therefore, the x j - and z j -axes are not unque, and the horzontal plane s ntroduced to solve ths problem Movng the horzontal plane to pont P j, the ntersectng lne between two planes s used to select the x j -axs, as shown n the rght part of Fgure 6 The ntersectng lne has two drectons Thus, the drecton s selected by determnng whether the angle between x j - and x o -axes s acute Afterward, the z j -axs s determned usng the rght-hand rule Fgure 7 shows the result of the local coordnates durng the entre phase-shftng process The smulaton wthout showng the coordnates s dscussed n the subsequent chapter

9 Appl Sc 218, 8, of 2 25 Plane vertcal to lnk vector Spral curve Vertcal vector Lnk vector Lnk z j y j P j x j P j Connecton pont Horzontal plane Fgure 6 Establshment of local coordnates Fgure 7 Local coordnates durng entre head-rasng process After the determnaton of the coordnate system, the relatve rotaton matrx between adjacent lnks can be obtaned usng Equaton (11) R P_relj = ( R Pj+1 ) 1 ( R Pj ) j = 2, 3, n = 1, 2,, n step [ ] r 11 r 12 r 13 R Pj = x j y j z j = r 21 r 22 r 23 r 31 r 32 r 33 (11) where R Pj R 3 3 s the orentaton matrx of lnk j relatve to the reference coordnate and R P_relj R 3 3 s the orentaton matrx of lnk j relatve to R Pj+1 Then, the relatve roll ( γ j ), ptch ( β j ), and yaw ( α j ) angle sequences can be calculated usng Equaton (12) β j = A tan 2( r 31, r r 21 2 ) j = 2, 3, n = 1, 2,, n step α j = Atan2(r 21 /cos( β j ), r 11 /cos( β j )) γ j = Atan2(r 32 /cos( β j ), r 33 /cos( β j )) (12)

10 Appl Sc 218, 8, of 2 The lne segments model for the snake robot s abstract, therefore we ntroduce a smplfed Soldworks model for the subsequent smulaton n Adams envronment, as shown n Fgure 8 It s part of Soldworks model and contans three modules Ths fgure provdes us wth the lne segments model (rght part) and Soldworks model (left part), and t s convenent for us to understandng where local coordnates are establshed and how the roll, ptch, and yaw angles are solved As mentoned before, a lne segment ( l j ) and a local coordnate ( P j x j y j z j ) represent a snake robot module, and a pont ( P j ) represents a jont wth three degrees of freedom The three axes are located at centers of rotatng shafts and ntersected at the P j Connectng the P j and the P j+1, the l j can be determned Equaton (11) and (12) provde us wth the soluton of roll, ptch and yaw angles However, t s sgnfcant to determne ther rotatng sequences, whch are also llustrated n Fgure 8 It should be ponted out that the rotatng sequences depend on specfc models, otherwse, the rotatng sequences and Equaton (12) should be modfed 1 P j + 1 Lnk j y j l j z j x j P j Lnk zj xj 1 yj Pj j 1 z j y j x j x j zj yj 2 γ j z j z j y x j xj j yj 3 α j y j z j x j yj zj 4 Roll angle γ Yaw angle α β j z j y j x j yj xj zj xj Ptch angle β Fgure 8 Local coordnates fxed on the smplfed Soldworks model and descrptons of roll, ptch and yaw angles Label 1 corresponds to the ntal confguraton Label 2, 3 and 4 descrbe the confguratons after rotatng roll angle, yaw angle and ptch angle, respectvely 3 Smulaton on Head-Rasng Moton Accordng to the proposed algorthms, the phase-shftng process can be smulated The parameters of the predefned curve are as follows: a = 97, b = 1, c = 485, n c = 25, n = 16, l lnk = 97 (mm), ˆt 1 = 1, 552 (rad), ˆt 2 = 1, 5551 (rad), ˆt 3 = 1, 5677 (rad), ϕ = 1622 (rad), and ϕ base = p (rad) Accordng to Equaton (3), the arc lengths of BS (S 1 ) and RS (S 2 ) can be calculated as S 1 = (mm) and S 2 = 1, 326 (mm) The dscrete ndependent varable s selected as t d = [, 1, 2,, ˆt 1, ˆt 1 + 1, ˆt 1 + 2,, ˆt 3 ] wth t 1 = 1 and t 2 = 1 To facltate readng, all the symbols utlzed n ths paper are concluded n Appendx part, as shown n Table A1

11 Appl Sc 218, 8, of 2 The snake begns to move from basc posture to fnal posture usng the shape-fttng and phase-shftng algorthms Durng the phase-shftng process, the phase S n every step can be selected manually In ths test, S s assumed constant and equal to (S 1 + S 2 )/n step, where the number of total steps n step equals 5 Table 1 shows the data of P 1, Inx p1, α 1, β 1, and γ 1 at steps 1, 2, 3, 499, and 5, respectvely Fgure 9 shows the phase-shftng process, ncludng the predefned spral curve (Fgure 9a) and snake posture at steps = 1 (Fgure 9b), = 147 (Fgure 9c), and = 5 (Fgure 9d), correspondng to the snake head at t = ˆt 1, t = ˆt 2, and t = ˆt 3, respectvely (a) (b) (c) (d) Fgure 9 Phase-shftng process: (a) predefned spral curve, (b) = 1, t = ˆt 1, (c) = 147, t = ˆt 2, (d) = 5, t = ˆt 3 We assume that the head-rasng moton tme s 5 s, that s, t tme (, 5) Wth Equaton (12) and numercal dervaton, the roll ( γ j ), ptch ( β j ), and yaw ( α j ) angle sequences; relatve angular velocty ( γ j, β j, α j ); and angular acceleraton ( γ j, β j, α j ) can be obtaned, as shown n the z-axs of Fgure 1 The x- and y-axes represent the jont ID (ID = 1, 2,, 15) and tme, respectvely As the snake head moves from t = ˆt 1 to t = ˆt 2, the absolute value of γ 1 ntally ncreases before the thrd jont pont moves to t = ˆt 1 and subsequently becomes constant because the frst two lnks are constraned to a semcrcle Meanwhle, the absolute values of α 1 and β 1 are zero Smlarly, the moton of the snake head movng from t = ˆt 2 to t = ˆt 3 can be dvded nto two stages Before the thrd jont pont moves to t = ˆt 2, the absolute value of γ 1 ntally decreases n the frst stage and then subsequently ncreases n the second stage In the meantme, the absolute value of α 1 ncreases at varyng speed The absolute value of β 1 ntally ncreases, subsequently decreases, and fnally ncreases Unsmooth angles are caused by the unsmooth transton of the pecewse functon, whch wll be mproved n future work Thus, the shapes of the relatve velocty and acceleraton curve are full of sharp ponts Moreover, the curves of the other jont ponts (ID = 2, 3,, 15) have the same shape wth phase lag

12 Appl Sc 218, 8, of 2 ptch angle(rad) ptch angle acceleraton(rad/s 2 ) yaw angle velocty(rad/s) jont ID jont ID jont ID 1 t= tˆ β j 4 tme(s) ɺɺ β 4 tme(s) γɺ 4 tme(s) j j ptch angle velocty(rad/s) yaw angle(rad) yaw angle acceleraton(rad/s 2 ) jont ID jont ID jont ID 1 2 t= tˆ ɺ βj 4 tme(s) γ j 4 tme(s) γɺɺ j 4 tme(s) roll angle(rad) -5-1 jont ID 1 t= tˆ 2 2 roll angle acceleraton(rad/s 2 ) tme(s) jont ID 1 α j 6 roll angle velocty(rad/s) jont ID 1 2 αɺɺ j 4 tme(s) αɺ j 4 tme(s) 6 Fgure 1 Angle, angular velocty, and angular acceleraton durng phase-shftng process

13 Appl Sc 218, 8, of 2 Step Head P 1 (mm) Table 1 Data durng phase-shftng process Index of Yaw Angle Ptch Angle Roll Angle Head Inx p1 α 1 (rad) β 1 (rad) γ 1 (rad) 1 (1518, 128, ) 15,52 2 (1513, 157, ) 15, (573, 618, 1881) 22, ( 546, 15, 667) 31, ( 36, 3, 694) 31, Dstngushng between the phase-shftng and head-rasng processes s mportant The phase-shftng process s used to obtan the jont trajectory and does not exst n realty f the snake robot has no actve wheels wth whch to move ts body along the predefned spral curve Meanwhle, the head-rasng process s the actual head-rasng moton produced usng the jont trajectory, and n ths case, the tal of the snake robot s fxed Fgure 11 shows the two knds of smulatons of head-rasng process Those smulatons are n knematc level and do not take dynamc factors nto consderaton The Smulaton n labels 1 5 are mplemented n Adams, and the roll, ptch and roll angles are ntegrated nto splne curves to gude the model s movement The smulaton n labels 6 9 are carred out usng Matlab, based on the followng algorthm: (1) Determne the basc posture as Equaton (6), and set moton step = 1; (2) Proceed to moton step = 2 The lnk n s fxed Calculate the poston of lnk n on the bass of angles 2 α n, 2 β n, and 2 γ n ; (3) Make an teraton to calculate all of the postons of the lnks, that s, lnk j = n 2, n 3,, 1, on the bass of angles ( 2 α n 2, 2 β n 2, 2 γ n 2 ), ( 2 α n 3, 2 β n 3, 2 γ n 3 ),, ( 2 α 1, 2 β 1, 2 γ 1 ); (4) Proceed to moton step = + 1 and repeat steps 2 and 3 untl = n step In ths algorthm, step 2 s mportant Durng the phase-shftng and head-rasng processes, ther lnk vectors l j, as shown n Fgure 5, are same n orentaton and dfferent n poston Therefore, the poston of jont pont P j can be obtaned as follows: l j 1 = l j R z ( α j )R y ( α j )R x ( α j ) P j = P j 1 + l j 1 R z ( α j ) = R z ( γ j ) = cos( α j ) sn( α j ) sn( α j ) cos( α j ) 1 1 cos( γ j ) sn( γ j ) sn( γ j ) cos( γ j ) R y ( β j ) = cos( β j ) sn( β j ) 1 sn( β j ) cos( β j ) (13) Consderng that the method of head-rasng should be adaptve to deal wth dfferent stuatons, more smulatons are mplemented as descrbed n Fgure 12, where dfferent parameters of the predefned spral curves are adopted and they are lsted n Table 2 In Fgure 12, only fnal postures are shown, and more detaled smulatons can be found n Supplementary Materals Compared wth the result n Fgures 11 and 12a, the radus of the supportng part BS becomes larger when ncreasng the value of parameter a (Fgure 12b d); the heght of every crcle becomes larger f the parameter c ncreases (Fgure 12b d) When changng the parameter b, the ampltudes of x s (t) and y s (t) change unequally Addtonally, the LP part changes accordngly (Fgure 12c,d) when changng the parameters l lnk and n, and t should be noted that ϕ s set to ensure the ntal phases of the sne and cosne functons All the smulatons confrm the feasblty and valdty of the proposed method

14 Appl Sc 218, 8, of Fgure 11 Head-rasng moton process Labels 1 5 show dfferent states from ntal posture to fnal posture of head-rasng n Adams envronment Label 1 s the ntal posture and label 5 s the fnal posture Label 2 corresponds to the case that the robot forms a supportng confguraton Label 3 and 4 are ntermedate states The labels 6 9 are head-rasng smulatons of lne segments model, mplemented n Matlab envronment The labels 6 9 correspond to labels 1, 2, 4 and 5, respectvely Table 2 Parameters of predefned spral curves used n head-rasng smulatons shown n Fgures 11 and 12 Fgure Number a b c n c ϕ (rad) n l lnk (mm) Fgure p Fgure 12a p Fgure 12b p Fgure 12c p 12 5 Fgure 12d p 12 5

15 Appl Sc 218, 8, of (a) (b) (c) (d) Fgure 12 Head-rasng moton process wth dfferent parameters of the predefned spral curve, and the parameters are lsted n Table 2 4 Dscusson and Conclusons In ths study, head-rasng moton of a snake robot based on a predefned spral curve s proposed and relatve smulatons are conducted Obtanng the angle sequences of the head-rasng moton of snake robots s dffcult under three-dmensonal condtons Thus, the proposed method adopts the predefned spral curve to gude the robot n movng, thereby mprovng the performance of the robot and layng the foundaton for accomplshng addtonal tasks The predefned spral curve s parameterzed Wth changes n the parameters, the curves can adapt to dfferent lengths and numbers of the snake robot modules, spral numbers, and heghts of head-rasng

16 Appl Sc 218, 8, of 2 However, there are some problems to be solved n the future work Frst, the predefned spral curve s a pecewse functon and has unsmooth ponts, thus leadng to sharp changes n moton trajectores Second, our model consders only knematc plannng, that s, the dynamc problem s not consdered Dynamc constrants are mportant factors for successful mplementaton of our model on a real robot, and wll be added nto an mproved model n the future work At the same tme, expermental verfcaton wll be carred out n the next phase of research, after fnshng the desgn and constructon of the platform The greatest challenge to use our model on a real robot s that the robot has to overcome the gravty durng the head-rasng, and the actuaton system must be equpped wth motors whch have adequate torques to drve the snake modules The work n [13] s a successful example although they adopted a dfferent method to rase the robot s head When desgnng the prototype, lghtweght materals can be used to enable the robot wth lghter gravty and stronger drvng power More mportantly, our model s adaptve As llustrated n Fgure 12, by decreasng the heght, the cycle number and ncreasng the radus of supportng part, t s feasble for the robot to rase ts head whle satsfyng the dynamc constrants In some specfc stuatons such as explorng the moon, and searchng for resources underwater, the gravty wll decrease or be compensated Then, t wll be easer for the robot to rase ther head Thrd, the head-rasng moton forms of snake n the natural world can change over tme To accomplsh that, the predefned spral curve should be made tme varyng wthout nfluencng the stablty of the robot body, and ths prncple wll also be the focus of the future research Supplementary Materals: The followng are avalable onlne at s1, Vdeo S1: Concept_of_trajectory_trackngav; t ntroduces the concept of trajectory trackng usng an mproved model n our future work Vdeo S2: PS_Fg9av; t descrbes the process of phase-shftng (Fgure 9) Vdeo S3: HR_Fg11av; t shows the process of head-rasng usng the lne segments model of the snake robot (Fgure 11) Vdeo S4: SA_Fg11av; t shows the smulaton of head-rasng n Adams envronment (Fgure 11) Vdeo S5: HR_Fg12aav; t dsplays the head-rasng process of Fgure 12a Vdeo S6: HR_Fg12bav; t dsplays the head-rasng process of Fgure 12b Vdeo S7: HR_Fg12cav; t dsplays the head-rasng process of Fgure 12c Vdeo S8: HR_Fg12dav; t dsplays the head-rasng process of Fgure 12d Author Contrbutons: Conceptualzaton, JL and ZJ; Formal analyss, XZ and JL; Fundng acquston, JL; Investgaton, JL, XZ, ZJ and CY; Methodology and Smulaton, XZ; Software and Programmng, XZ; Supervson, JL, ZJ and CY; Valdaton, JL; Wrtng orgnal draft, XZ; Wrtng revew and edtng, ZJ, CY and JL Fundng: Ths work was partally supported by the Natonal Scence Foundaton of Chna (Grant No ), Research Fund of Chna Manned Space Engneerng (Grant No 321), Engneerng and Physcal Scences Research Councl (EPSRC) (Grant No EP/S1913/1) And the APC was funded by the Natonal Scence Foundaton of Chna (Grant No ) Conflcts of Interest: The authors declare no conflct of nterest Abbrevatons The followng abbrevatons are used n ths manuscrpt: 2D 3D Two dmensonal Three dmensonal

17 Appl Sc 218, 8, of 2 Appendx A Table A1 shows all the symbols utlzed n ths paper Symbol Defnton Table A1 Symbols utlzed n ths paper x l (t) x value of lne part n the predefned spral curve y l (t) y value of lne part n the predefned spral curve z l (t) z value of lne part n the predefned spral curve x s (t) x value of spral curve part n the predefned spral curve y s (t) y value of spral curve part n the predefned spral curve z s (t) z value of spral curve part n the predefned spral curve a Adjustment coeffcent of the spral curve, whch can be used to change the ampltude of x s (t) and y s (t) equally b Adjustment coeffcent of the spral curve, whch can be used to change the ampltude of x s (t) and y s (t) unequally c Adjustment coeffcent of the spral curve, whch can be used to change the ampltude of z s (t) ϕ Intal phase of the sne and cosne functons n c Cycle number of the spral curve l lnk Length of the snake robot modules n Number of the snake robot modules t Independent varable dvded nto three ntervals: (, ˆt 1 ), (ˆt 1, ˆt 2 ) and (ˆt 2, ˆt 3 ) y s (t) y value of spral curve part n the predefned spral curve LP Lne part of the predefned spral curve, correspondng to nterval (, ˆt 1 ) BS The base of the spral curve part, whch s n contact wth the ground and corresponds to nterval (ˆt 1, ˆt 2 ) RS y The base of the spral curve part, whch s n contact wth the ground and corresponds to nterval (ˆt 2, ˆt 3 ) ˆt 1 Value of t connectng LP and BS ˆt 2 Value of t connectng BS and RS ˆt 3 End value of t ϕ base The phase value of nterval (ˆt 1, ˆt 2 ) t y Dscrete value of smulaton tme Index value of smulaton step j Index value of pont P n lne segments model of the snake robot P j Poston of pont j step of phase-shftng process n step Number of phases shfted x snk_j x value of P j y snk_j y value of P j

18 Appl Sc 218, 8, of 2 Table A1 Cont Symbol Defnton z snk_j z value of P j S Arc length at step and defned as phase P snk Pont set at step t d y Dscrete vector of t t 1 Step length of dscretzaton n nterval (, ˆt 1 ) t 2 Step length of dscretzaton n nterval (ˆt 1, ˆt 3 ) Inx pj Index value of element n t d, whch s nearest to t +1 durng phase-shftng process eps Threshold utlzed to deal wth the error between contnuous functon and dscrete approxmaton O xo y o z o Reference coordnate x j x coordnate axs fxed on the snake module j at step y j y coordnate axs fxed on the snake module j at step z j z coordnate axs fxed on the snake module j at step l j Lnk vector connectng P j and P j+1 P j x j y j z j y Local coordnate fxed on lnk j α j Yaw angle of jont j at step β j Ptch angle of jont j at step γ j Roll angle of jont j at step α j Yaw angular velocty of jont j at step β j Ptch angular velocty of jont j at step γ j Roll angular velocty of jont j at step α j Yaw angular acceleraton of jont j at step β j Ptch angular acceleraton of jont j at step γ j Roll angular acceleraton of jont j at step t tme Head-rasng moton tme R Pj Orentaton matrx of lnk j wth respect to the reference coordnate at step R P_relj Orentaton matrx of lnk j wth respect to R Pj+1 at step R x () Matrx nvolvng vector rotatng around the x axs R y () Matrx nvolvng vector rotatng around the y axs R z () Matrx nvolvng vector rotatng around the z axs

19 Appl Sc 218, 8, of 2 References 1 Transeth, AA; Pettersen, KY; Lljebäck, P A survey on snake robot modelng and locomoton Robotca 29, 27, [CrossRef] 2 Lljebäck, P; Pettersen, KY; Stavdahl, Ø; Gravdahl, JT A revew on modellng, mplementaton, and control of snake robots Robot Auton Syst 212, 6, Lu, J; Wang, Y; L, B; Ma, S Path plannng of a snake-lke robot based on serpenod curve and genetc algorthms In Proceedngs of the 5th World Congress on Intellgent Control and Automaton, Hangzhou, Chna, June 24; pp Sato, M; Fukaya, M; Iwasak, T Serpentne locomoton wth robotc snakes IEEE Control Syst Mag 22, 22, [CrossRef] 5 Chen, L; Wang, Y; Ma, S; L, B Analyss of travelng wave locomoton of snake robot In Proceedngs of the 23 IEEE Internatonal Conference on Robotcs, Intellgent Systems and Sgnal Processng, Changsha, Chna, 8 13 October 23; pp Kalan, H; Akbarzadeh, A; Safehan, J Travelng wave locomoton of snake robot along symmetrcal and unsymmetrcal body shapes In Proceedngs of the 41st Internatonal Symposum on Robotcs and 6th German Conference on Robotcs, Munch, Germany, 7 9 June 21; pp Lu, J; Wang, Y; L, B; Chen, L; Ma, S Serpentne locomoton wth robotc snakes Chn J Mech Eng 25, 41, [CrossRef] 8 Vrgala, I; Dovca, M; Kelemen, M; Prada, E; Bobovsky Snake robot movement n the ppe usng concertna locomoton Jxe Gongcheng Xuebao/Chn J Mech Eng 25, 41, [CrossRef] 9 Chen, L; Wang, Y; Ma, S; L, B Study of lateral locomoton of snake robot Robot 23, 25, Tanev, I; Ray, T; Buller, A Evoluton, robustness, and adaptaton of sdewndng locomoton of smulated snake-lke robot In Genetc and Evolutonary Computaton Conference; Sprnger: Berln, Germany, 24; pp Wang, K; Gao, W; Ma, S Snake-lke robot wth fuson gat for hgh envronmental adaptablty: Desgn, modelng, and experment Appl Sc 217, 7, 1133 [CrossRef] 12 Sanflppo, F; Azpazu, J; Marafot, G; Transeth AA; Stavdahl, Ø; Lljebäck, P Study of lateral locomoton of snake robot Appl Sc 217, 7, 336 [CrossRef] 13 Ye, C; Ma, S; L, B; Wang, Y Head-rasng moton of snake-lke robots In Proceedngs of the 24 IEEE Internatonal Conference on Robotcs and Bommetcs, Shenyang, Chna, August 24; pp Tanaka, M; Matsuno, F Modelng and control of head rasng snake robots by usng knematc redundancy J Intell Robot Syst 214, 75, [CrossRef] 15 Cappo, EA; Choset, H Plannng end effector trajectores for a serally lnked, floatng-base robot wth changng support polygon In Proceedngs of the 24 Amercan Control Conference, Portland, OR, USA, 4 6 June 214; pp Wu, W; Hong, Y; Wang, G Geometrcal splne approach to shape control of super redundant planar manpulator Mach Tool Hydraul 24, 4, Nor, NM; Ma, S Body shape control of a snake-lke robot based on phase oscllator network In Proceedngs of the 213 IEEE Internatonal Conference on Robotcs and Bommetcs, Shenzhen, Chna, December 213; pp Lljebäck, P; Pettersen, KY; Stavdahl, O; Gravdahl, JT A control framework for snake robot locomoton based on shape control ponts nterconnected by Bézer curves In Proceedngs of the 212 IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems, Algarve, Portugal, 7 12 October 212; pp Tanaka, M; Tanaka, K Shape Control of a snake robot wth jont lmt and self-collson avodance IEEE Trans Control Syst Technol 217, 25, [CrossRef] 2 Lu, J; Wang, Y; Ma, S; L, B Shape control of hyper-redundant modularzed manpulator usng varable structure regular polygon In Proceedngs of the 24 IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems, Senda, Japan, 28 September 2 October 24; pp Chrkjan, GS; Burdck, JW A modal approach to hyper-redundant manpulator knematcs IEEE Trans Robot Autom 1994, 1, [CrossRef]

20 Appl Sc 218, 8, of 2 22 Burdck, JW; Radford, J; Chrkjan, GS A sdewndng locomoton gat for hyper-redundant robots Adv Robot 1995, 9, [CrossRef] 23 Hatton, RL; Choset, H Generatng gats for snake robots by annealed chan fttng and keyframe wave extracton Auton Robot 21, 28, [CrossRef] 24 Tavakkol, S; Dhande, SG Shape synthess and optmzaton usng ntrnsc geometry J Mech Des 1991, 113, [CrossRef] 25 Mochyama, H; Shmemura, E; Kobayash, H Shape control of manpulators wth hyper degrees of freedom Int J Robot Res 1999, 18, [CrossRef] c 218 by the authors Lcensee MDPI, Basel, Swtzerland Ths artcle s an open access artcle dstrbuted under the terms and condtons of the Creatve Commons Attrbuton (CC BY) lcense (