KINEMATICS AND IMPLEMENTATION OF

CONTINUUM MANIPULATORS

_________________

A Dissertation

Presented to

the Graduate School of

Clemson University

_________________

In Partial Fulfillment

of the Requirements for the Degree

Doctor of Philosophy

Electrical Engineering

_________________

by

Bryan A. Jones

August 2005

Advisor: Dr. Ian D. Walker

ABSTRACT

We introduce a new method for synthesizing kinematic relationships for a general

class of continuous backbone, or continuum, robots. The resulting kinematics enable real-

time task and shape control by relating work space (Cartesian) coordinates to actuator

inputs, such as tendon lengths or pneumatic pressures, via robot shape coordinates. This

novel approach, which carefully considers physical manipulator constraints, avoids arti-

facts of simplifying assumptions associated with previous approaches, such as the need to

fit the resulting solutions to the physical robot. It is applicable to a wide class of existing

continuum robots and models extension, as well as bending, of individual sections. In ad-

dition, this approach produces correct results for orientation, in contrast to previously

published approaches. Results of real-time implementations on two types of spatial multi-

section continuum manipulators are reported.

ACKNOWLEDGEMENTS

My graduate studies at Clemson have truly been a pleasure for me. Dr. Walker’s

rapid feedback on paper revisions and patience as I explored these ideas has been a bless-

ing. I appreciate his advice, mentoring, and support and look forward to my career in

academia. My sweet wife Vika has been such a blessing to me, consistently encouraging

me as I struggled to develop new ideas and communicate them to others. I’m so thankful

for her love and support. My family, beginning with science projects while I was in ele-

mentary school, has constantly supported and helped developed my interest in discovery

and engineering; I am very thankful. I’m thankful for the Lord’s constant blessing as I

pursued unknown roads and traveled to new places. Many others have been a pleasure to

work with, including the Control and Robotics group at Clemson and Penn State me-

chanical engineering team. I appreciate their help and enthusiasm. Finally, the support

of DARPA through Contract N66001-C-8043 has enabled me to pursue this research.

TABLE OF CONTENTS

Page

TITLE PAGE.......................................................................................................................i

ABSTRACT........................................................................................................................ii

ACKNOWLEDGEMENTS...............................................................................................iii

TABLE OF CONTENTS...................................................................................................iv

LIST OF TABLES.............................................................................................................vi

LIST OF FIGURES..........................................................................................................vii

CHAPTER

1. INTRODUCTION...................................................................................................1

2. TRUNK CABLE KINEMATICS............................................................................6

Transformation from cable lengths to trunk curvature.....................................8

Transformation from trunk curvature to cable lengths...................................22

3. TRUNK KINEMATICS.........................................................................................30

Trunk modeling and mechanics......................................................................31

Positional Kinematics.....................................................................................37

Velocity Kinematics........................................................................................52

Discussion and implementation......................................................................58

4. IMPLEMENTATION.............................................................................................64

Limiting case analysis.....................................................................................69

Effects of actuator length limits......................................................................81

Workspace geometry.......................................................................................97

Joint limit avoidance.......................................................................................99

Tangle/Untangle............................................................................................104

Implementation results...................................................................................111

v

Table of Contents (Continued) Page

5. RAPID PROTOTYPING.....................................................................................116

Enabling technologies...................................................................................118

Multiple configuration support.....................................................................136

6. RESULTS AND CONCLUSIONS......................................................................148

APPENDIX......................................................................................................................153

A. RAPID PROTOTYPING SUPPLEMENTARY INFORMATION......................153

Installation and use of C++ capabilities for RTW........................................153

Installation procedure for QMotor build environment..................................156

BIBLIOGRAPHY............................................................................................................158

LIST OF TABLES

Table Page

3.1. Hannan’s D-H table in 2-D. Asterisks mark joint variables. Link 1, which

extends the table to 3-D, is not present..................................................................40

3.2. Hannan’s 3-D D-H table........................................................................................42

3.3. Modified D-H table with anti-torsional term.........................................................42

3.4. New D-H table, where asterisks represent joint variables.....................................47

3.5. New D-H table with additional links to orient trunk extension along the z

axis.........................................................................................................................51

LIST OF FIGURES

Figure Page

1.1. Two continuum robots studied in this work, (a) OctArm and (b) Air-Octor...........1

2.1. Trunk variables

s

,

φ

Ⱐ慮搠

κ

⸠.周攠癡物慢汥θ

φ

楶敳⁴桥潴慴楯渠楮⁴桥=

x

y

plane.........................................................................................................................8

2.2. Bent segment in a section of the trunk.....................................................................9

2.3. Top portion of bent trunk segment, showing heights measured above an

imaginary plane......................................................................................................10

2.4. Top-down view of trunk.........................................................................................12

2.5. Height of points 1 and 2 above plane.....................................................................13

2.6. Geometry of bent trunk segment............................................................................15

2.7. Determination of

i

r

.................................................................................................16

2.8. Transformation of basis from the set

{

}

ㄲ 3

Ⱜi i i

to the set

{

}

,

x y

i i

.............................18

2.9. Cables bend only at cable guides...........................................................................20

2.10. Computing

s

...........................................................................................................21

2.11. Plane containing circle and reference plane...........................................................23

2.12. Rotated view of Figure 2.11...................................................................................24

2.13. Computing

c

h

from

β

⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸲㔠

′⸱㐮= 䍯C灵瑩湧p

a

in Figure 2.12...................................................................................26

2.15. Line used to derive slope and intercept..................................................................27

2.16. Determining cable length.......................................................................................28

3.1. The Air-Octor two-section continuum robot..........................................................33

3.2. The OctArm four-section continuum robot...........................................................34

viii

List of Figures (Continued)

Figure Page

3.3. D-H joints in Hannan’s 2-D table. Revolute joints are marked by circles

and prismatic joints by boxes.................................................................................39

3.4. Geometric transformation for Hannan’s 2-D D-H table........................................41

3.5. The orientation of Hannan’s trunk, showing the torsional component built

into the D-H table, illustrated by graphing the trunk as it rotates through

the range

3

0

2

φ

π

⎡ ⎤

=

⎢ ⎥

⎣ ⎦

…

..........................................................................................44

3.6. The orientation of the new trunk, showing the lack of torsional component

in the D-H table, illustrated by graphing the trunk as it rotates through the

range

3

0

2

φ

π

⎡ ⎤

=

⎢ ⎥

⎣ ⎦

…

................................................................................................45

3.7. New D-H table. The trunk modeled by this table is shows as a thick,

semi-transparent line..............................................................................................46

3.8. Coordinate axes for D-H table given in Table 3.4, where the circled U

shows an axis extending upward from the page....................................................48

3.9. Determining a geometrical transformation between D-H parameters and

trunk parameters. The semi-transparent line shows the actual trunk section

modeled by the geometry.......................................................................................49

4.1. Intrinsic actuation scheme. Three actuators are connected in parallel

between two plates. Differences in length cause bending of the structure...........67

4.2. Extrinsic and hybrid actuation schemes. Three tendons, labeled cable 1 to

3, shape the robot. In the center, a passive backbone in the extrinsic case

or a flexible actuator such as pneumatically pressurized tube in the hybrid

case.........................................................................................................................68

4.3. A plot of

( ) ( )

(

)

1 2 3 1 2 3

0

F

,,lim,,

g

l l l l l l

→

+ ∆ −J J

, showing that the high-

precision (to 60 decimal digits) Jacobian accurately approximates the

analytically-determined limiting-case Jacobian for

19

10

−

∆

>

................................76

4.4. Comparison of the error

(

)

(

)

(

)

60 1 2 3 14 1 1 2 3 2 3

F

,,,,,,,

p

l l l l l l l l l

δ+ ∆ − + ∆J J

over a range of differences

∆

渠捡扬攠汥湧瑨潲⁴睯散瑩潮⁴牵湫猠瑨==

←i湩←u洠慬汯睡扬攠捡扬攠獱畡牥搠汥湧瑨楦∞敲敮捥⁴敲e=

δ

桡湧敳⸠=周攠

畳攠潦u

1

p

l

as defined by (59) guarantees this minimum..........................................77

ix

List of Figures (Continued)

Figure Page

4.5. Comparison of Figure 4.4 repeated for a single-section trunk...............................79

4.6. Comparison of Figure 4.4 repeated for a three-section trunk................................80

4.7. Comparison of Figure 4.4 repeated for a one-section trunk, but defining

1

p

l

as the lesser of the two solutions of (58) reveals numerical stability

problems.................................................................................................................81

4.8. Example of curvature and angle of wrap. The curvature is

0.1

κ

=

and

angle of wrap

90

θ

= °

.............................................................................................84

4.9. Examination of the extrema of

κ

猠摥晩湥搠批
㘰⤮†䅳A

3

l

varies from

27.5 to 35, the extrema move from

(

)

1 2

,l l

=

(

)

㌵,25

⁴漠

(

)

25,25

when

3

35

l

=

. The plots were produced with

(

)

(

)

min max

,25,35l l

=

cm and a trunk

radius

1d =

cm.......................................................................................................85

4.10. Varying trunk length

(

)

max

s

κ

φ

, determined by evaluating (70), necessary to

achieve maximum curvature illustrated in Figure 4.11, versus fixed trunk

lengths which achieve maximum curvature at certain locations. Trunk

minimum actuator length was

min

25

l

=

←湤a硩←u←捴畡瑯爠汥湧瑨=

睡猠

浡←

㌵

l

=

cm for a trunk of radius 1d

=

cm......................................................89

4.11. Trunk maximum curvature

(

)

max

κ

φ

determined by evaluating (71), shown

in a dotted red line as the angle of curvature

φ

⁶慲楥献†周攠摡獨敤汵攠汩湥θ

慮搠獯汩搠杲敥渠汩湥汬畳≥±a瑥⁴≥ 攠敦∞散瑳∞i硩湧⁴牵湫敮杴n=

s

,

determined by evaluating (73). Trunk minimum actuator length was

min

25

l

=

cm and maximum actuator length was

max

35

l

=

=潲⁴牵湫==

牡摩畳‱ d = cm.......................................................................................................90

4.12. Trunk maximum wrap angle

(

)

max

θ

φ

determined by evaluating (72),

shown in a dotted red line as the angle of curvature

φ

⁶慲楥献†周攠摡獨敤θ

扬略楮攠慮搠獯汩搠杲敥渠汩≤攠楬汵獴牡瑥 ⁴桥=晥捴猠潦楸楮∞⁴牵湫敮杴栠

s

,

determined by evaluating (73). Trunk minimum actuator length was

min

25

l

=

cm and maximum actuator length was

max

35

l

=

=潲⁴牵湫==

牡摩畳‱ d = cm.......................................................................................................91

4.13. Maximum achievable trunk curvature

(

)

浡x

,s

κ

φ

at a given angle of

curvature

φ

湤⁴牵湫敮杴栠

s

computed by applying (73). Results were

obtained for a trunk where maximum an minimum actuator lengths are

max

l

= 35 cm,

min

l

= 25 cm and the trunk radius is

1d

=

cm..........................................92

x

List of Figures (Continued)

Figure Page

4.14. A plot of the surface formed by rotating the trunk, drawn at 60

φ

= ° from

[

]

30,150

φ

= ° °

. Plot (a) was formed by choosing

(

)

max

s

s

κ

φ

=

from (70) and

( )

max

κ

κ φ=

from (71). Plot (b) was formed by fixing

s

to achieve

max

κ

==

摥晩湥搠楮
㘲⤬湤⁰汯琠⡣⤠批楸楮朠

s

to achieve

2max

κ

猠摥晩湥搠批
㘳⤮==

周攠瑲畮欠扥湤猠楮瑯楲捬攠潦⁶慲θ 楮朠牡摩畳慳敤渠慣瑵慴潲敮杴栠

汩l楴猠潦=

浩n

㈵

l

=

cm and

max

35

l

=

=⸠.θ±畮欠摩慭整敲猠

1d =

cm.....................94

4.15. Plot of

min

s

and

max

s

determined by evaluating (74) over

φ

湤=

κ

⸠.周攠瑷漠

扥杩渠瑯癥牬慰敡爠

浡←

κ

Ⱐ楮摩捡瑩湧⁴桡琠,a硩xu 洠捵牶慴畲攠捡渠潮汹攠

潢瑡楮敤琠愠獰散楦楣⁴牵湫敮杴栠

(

)

max

s

κ

φ

. Results were obtained for a

trunk where maximum an minimum actuator lengths are

max

l

= 35 cm,

min

l

= 25 cm and the trunk radius is 1d

=

cm...............................................................96

4.16. Workspace of a single-section robot drawn from

[

]

㌰,ㄵ1

φ

=

° °

with trunk

radius 1d

=

cm in (a) and 2.25d

=

cm in (b). Actuator length limits are

(

)

㈵,35

=渠扯瑨慳敳=⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸹㠠

=㐮ㄷ⸠ 偯獴畲攠潦⁴牵湫敦潲攠⡰潳楴楯渠䄬A 汥晴⁴牵湫⤠慮搠慦瑥爠⡰潳楴楯渠䈬楧桴≤

瑲畮欩⁴桥潩湴業楴癯楤慮捥i 杯物瑨gs潮癥= g敳⸠乯瑥⁴桡琠瑨攠

汥湧瑨猠潦⁴潰湤潴瑯l散瑩潮猠慲 攠敱畡氠慴″〠捭晴敲⁴桥汧e物瑨洠

捯湶敲g敳Ⱐ慬汯眠e潶→浥湴Ⱐ睨楬攠橯楮琠汩 ←楴猠扥景牥牥琠瑨攠橯楮琠汩i楴猠

潦
㈵←Ⱐ㌵,⤮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮ㄠ

=㐮ㄸ⸠A散潮搠慣瑵慴潲敮杴栠汩=楴硡浰汥⸠iA癯v搠慰灬祩湧⁴桥癯楤慮捥≤

慬杯a楴桭,⁴楰⁰潳楴楯渠牥= a楮猠瑨i慭e⁷h楬攠i潶楮朠慷慹±潭⁴桥=

←a硩←u洠←c瑵慴≥±敮=瑨渠扯瑨散瑩潮猠潦⁴桥⁴牵湫≥⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮㌠

=㐮ㄹ⸠ 剥獵汴猠潦灰汹楮朠瑨攠畮瑡湧汩湧R 慬杯物瑨a⁴漠愠瑷漭獥捴楯渠䅩A-佣瑯O,=

捯cp慲楮朠a敡獵牥搠瑲畮欠灯獩瑩e渠獨潷 渠潮⁴桥敦琠睩瑨湤⁷楴桯畴n

畮瑡湧汩湧潭p慲敤⁴漠瑨攠牯扯琠晲 潭⁷桩捨⁴桥敡獵牥e敮瑳⁷敲攠

瑡步渮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮㔠

=㐮㈰⸠ 䱯捡瑩潮映瑨攠䅩A-佣瑯爠捡扬敳⁷桩捨捴畡瑥瑳⁴睯散瑩±湳n⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮㜠

=㐮㈱⸠ 䉬潣欠摩慧牡洠潦潢潴 潮瑲潬祳瑥洮†啳敲湰畴 猠牥慤牯洠愠橯祳瑩捫Ⱐ

瑨敮⁰慳獥搠瑯⁴桥≥ 琠潦潮瑩湵畭潢潴楮敭a瑩挠敱畡瑩潮献†周攠

潵瑰畴映瑨攠歩湥→a瑩捳汯≥ 欠楳⁴桥n⁵獥搠瑯=湴牯氠瑨n潢→琬≥整=楬i≤=

楮⁆楧畲攠㐮㈲湤⁆楧畲攠㐮㈳i⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮1ㄲ1

=㐮㈲⸠ 䑥瑡楬猠潦⁴桥潢潴l潣欠潦⁆楧畲攠㐮㈱潲⁴桥⁏捴䅲→⁴牵湫=⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮.1ㄳ1

=㐮㈳⸠ 䑥瑡楬猠潦⁴桥潢潴l潣欠潦→ 䙩杵牥‴⸲〠景爠瑨攠䅩A-佣瑯爠瑲畮欮 ⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮⸮1ㄴ1

xi

List of Figures (Continued)

Figure Page

5.1. Overall flow of the rapid prototyping tools used for continuum robot

development.........................................................................................................117

5.2. The joystick Simulink block and its block parameters dialog box......................134

5.3. The network client Simulink block and its block parameters dialog box............136

5.4. The network server Simulink block and its block parameters dialog box...........137

5.5. Simulink I/O blocks. For brevity, only the block parameters of the analog

to digital converter block is shown......................................................................138

CHAPTER 1

INTRODUCTION

Continuum robots, in contrast to traditional rigid link robots, feature a continuous

backbone with no joints [1], as shown in Figure 1.1. In this sense, they are essentially

invertebrate, as opposed to the vertebrate-like structure of conventional robots. The wide

range of abilities demonstrated by invertebrate limbs such as elephant trunks [2], [3], [4],

octopus arms [5], [6], and squid tentacles [7] have motivated a recent surge of research

activity in continuum robots. Potential applications include navigation and operation in-

side complex, congested environments, such as collapsed buildings in search and rescue

operations [8], [9], [10] or the human body in medical applications [11], [12], [13]. Con-

tinuum robots also enable novel forms of environmental interaction, via compliant and/or

whole arm manipulation [14].

Realization of continuum robots proves to be a very interesting and challenging

(a) (b)

Figure 1.1 Two continuum robots studied in this work, (a) OctArm and (b) Air-Octor.

2

problem. A large proportion of efforts in the area have focused on design and construc-

tion of continuum robot hardware. To date, a number of interesting hardware prototypes

have been produced [2], [4], [5], [14], [15], [16], [17], [18], [19], [20], [21], as well as

several commercial products [22], [23]. These manipulators demonstrate the strong po-

tential of continuum robots. In comparison, efforts in modeling and real-time algorithm

development to extract the full physical potential from these robots have lagged behind.

For example, almost all previous approaches either do not address trunk orienta-

tion and either approximate positional kinematics or solve them only for limited kine-

matic models, which constrain the performance of continuum robot hardware to reduced

classes of shapes and motions, restricting their potential applicability. In this work, we

modify and extend some of the ideas in [3] as part of a new and quite general kinematic

formulation for continuum robots. This approach, unlike previous work, produces com-

plete and correct results for both task-space position and orientation. The method fully

models for the first time, to the best of our knowledge, the exact kinematics of trunk sec-

tions which can extend in length as well as bend in two dimensions using closed-form

equations. The approach is also modular, allowing its application to a wide range of

physical realizations, including in particular the common three evenly-spaced actuators

per section design. In addition, the resulting Jacobians, derived via a straightforward ap-

plication of well-known Denavit-Hartenberg (D-H) techniques and the chain rule, are

relatively simple and computable in real time. This model also allows a straightforward

analysis of singularities present in a continuum manipulator.

However, development of a theoretical foundation for continuum kinematics must

be accompanied by solutions amenable to real-time implementation of these kinematics.

3

Singularities and numerical instabilities near singular configurations, for example, pre-

vent computation of the end-effector position or velocity in those configurations, causing

control code to produce excessive and perhaps dangerous actuator torques. Actuator

length limits, though not present or accounted for in the kinematics developed previously,

shape the achievable configuration space of the robot in unexpected ways due to the cou-

pling present in continuum robots. To achieve full use of continuum robots, these cou-

plings must be understood and exploited. This work introduces a new and complete prac-

tical solution for the inclusion of actuator length limits for continuum robot kinematics.

Finally, the challenges of designing a software environment to support rapid de-

velopment of a mobile, continuum manipulator are great, due to the diversity of platforms

and environments necessary for robot development. Such an environment should support

development in simulation on a desktop PC, testing and debugging in the laboratory us-

ing the actual robot, and field operation via wireless control and monitoring of a contin-

uum robot mounted on a mobile platform. In addition, a methodology to both compute

the complex kinematics developed for the robot and transform these expressions into

real-time code is necessary. Finally, a visualization tool for viewing the posture of a con-

tinuum trunk in 3D is essential for understanding the operation of the trunk. This work

also presents a novel rapid prototyping environment for continuum robots which meets

all of these challenges.

Chapter 2 begins the derivation of kinematics for continuum robots. First, it es-

tablishes a unique parameterization for continuum robots which is independent of their

actuation strategy (cable-driven or pneumatically-shaped) in terms of trunk length

s

, cur-

vature

κ

Ⱐ慮搠慮杬攠潦畲癡瑵牥,

φ

猠楬汵獴牡≥敤渠䙩杵牥′⸱⸠⁆牯e⁴桩猠業灬敭敮瑡瑩e渭

4

neutral parameterization, forward and inverse kinematics are for the first time developed

which map actuator lengths to trunk parameters

s

,

κ

Ⱐ慮搠

φ

潲⁴睯牯慤慴敧潲楥猠潦=

捯湴楮畵c潢潴猠⡩湴物=s楣湤i 數瑲楮獩振桹e物搬猠摥晩湥搠楮 ⁛ㅝ⤮†䱩←楴楮朠捡獥猠楮桥爭

敮琠楮⁴桥e獵汴楮朠數灲敳獩潮猠e±攠el獯 硡=楮敤湤潬癥i湡汹瑩捡汬=.=

䍨慰瑥爠㌠捯Cp汥瑥猠瑨攠歩湥ea瑩捳敲≥ va瑩潮礠牥污瑩湧⁴牵湫⁴楰⁰=獩瑩潮湤=

潲楥湴慴楯渠

x

and its linear and angular velocity

x

to trunk parameters

s

,

κ

Ⱐ慮搠

φ

爠瑯=

s

,

κ

, and

φ

. The resulting homogenous transformation matrix

A

and Jacobian

J

are devel-

oped using standard D- H techniques coupled with a uniqu e geometrical transformation to

s

,

κ

Ⱐ慮搠

φ

湤⁰牯摵捥潲牥捴敳畬瑳潲潴栠灯= 楴楯渠慮搠潲楥湴慴楯測渠捯湴牡獴⁴漠

灲敶楯畳=灲潡捨敳p†䥮捯牰潲慴楮=敳=汴猠晲l←桡灴敲′=⁴桥獥敳=汴猠慲攠摥物癥搠楮l

湯癥氬潤畬慲慳桩潮→爠扯瑨⁴祰敳映捯湴楮 畵u潢潴猠楮癥獴楧慴敤 渠瑨慴桡灴敲,=

祩敬摩湧煵慴楯湳畩瑡扬攠景爠牥慬業攠e 潮瑲潬映扯瑨污獳敳映捯湴楮畵→潢潴献=

䡯睥癥H,浰汥=敮瑡瑩潮⁰牯扬敭s潴灰= 牥湴牯±⁴桥楮敭a瑩捳畳琠晩牳琠

扥摤牥獳敤敦潲攠瑨敳攠數灲敳獩潮猠捡渠扥b 畳敤⸠⁃桡灴敲‴摤牥獳敳湤潬癥猠瑨敳攠

灲潢汥p献†䙩牳琬楮杵污物瑩敳湤畭敲 楣慬湳瑡扩汩瑩敳⁰牥獥湴渠扯瑨⁴桥i A and

J

ma-

trices are identified and analyzed. This ne w analysis reveals that perturbing one cable

length for particular cases prevents thes e problems and provides a methodology which

performs this perturbation when necessary based on a simple expression which can be

easily evaluated in real-time. Next, the previously unknown inter action between trunk

parameters

s

,

κ

Ⱐ慮搠

φ

略⁴漠慣瑵慴潲敮杴栠汩=楴猠慲攠 慮慬祺敤湤畭扥爠潦硰牥b-

獩潮猠睨楣栠摥晩湥⁴桥獥湴s 牡捴楯湳湤⁰牯癩摥e瑨潤≥ 瑯硴牡捴a硩浵x牡獰楮朠

灥牦潲pa湣攠晲潭潮瑩湵畭⁴牵湫敳瑲楣= 敤礠慣瑵慴潲敮杴栠汩e楴猠楳⁰牥獥湴敤i==

䙲潭⁴桩猠慮慬祳楳=⁴桥⁷潲歳灡捥∞潮瑩 湵n←⁴牵湫猠慮慬祺敤湤⁰汯瑴敤⸠⁆楮慬汹,=

5

a unique tangle/untangle algorithm which correctly compensates for coupling in distal

(tip) sections due to the shape of proximal (base) sections is derived and presented, in-

cluding results of implementing this algorithm on a continuum trunk.

The work of chapters 2 to 4 is then implemented in real-time on two different

types of continuum trunks using the novel rapid-prototyping software development envi-

ronment presented in chapter 5. An original system for deriving the kinematics presented

in chapters 2 and 3 and converting the symbolically-computed results into C code suited

for real-time execution is detailed. A 3D modeling system uniquely suited to displaying

the posture of a continuum trunk in real-time is discussed. The rapid-prototyping envi-

ronment includes support for multiple configurations, enabling development of algo-

rithms for continuum trunks in simulation on a desktop PC, in the laboratory on a contin-

uum trunk, or in the field, remotely controlling a trunk mounted on a mobile platform via

a wireless link. A new build process which allows transforming a block diagram devel-

oped in Simulink into a real-time executable which runs on the QNX real-time operating

system is reviewed. In addition, a set of blocks which perform necessary functions of

communicating over a network connection, reading user input from a joystick, and inter-

facing with robot hardware through encoders, A/D converters, and D/A converters is pre-

sented. The final chapter concludes by reviewing the results obtained in this work and

examining future directions for the continued development of continuum trunks.

CHAPTER 2

TRUNK CABLE KINEMATICS

In spite of the impressive abilities demonstrated by the large variety of continuum

robots already built, efforts in modeling and real-time algorithm development to extract

the full physical potential from these robots have lagged behind. For example, almost all

previous approaches either do not address trunk orientation, approximate positional

kinematics, or solve them only for limited kinematic models. This constrains the per-

formance of continuum robot hardware to reduced classes of shapes and motions, restrict-

ing their potential applicability.

Hirose and colleagues designed the serpeniod curve, which closely matches the

kinematics of a snake’s body in 2-D as it crawls across the ground [24]. However, this

work does not address the 3-D kinematics of the many snake-like robots they designed;

instead, it focuses on their planar dynamics. Chirikjian and Burdick took the opposite ap-

proach, in which they link (approximate) the shape of a general mathematical curve to a

high degree of freedom robot [25], [26], [27]. Their modal decomposition then reduced

the number of degrees of freedom necessary to control the curve to which the robot was

fitted, reducing the computational complexity of the problem but typically sacrificing full

control of all degrees of freedom within the robot structure. Although recent work [28]

extends this approach by introducing a more convenient curve, this method relies on ap-

proximating the shape of an actual robot which leads to a number of kinematic difficul-

ties, such as mode switching, inherent in that approach. An alternate approach is to ex-

7

actly compute the kinematics of a high degree of freedom manipulator composed of

spherical joints and rigid links using standard D-H methods [10]; however, this approach

cannot be applied to continuum robots, which contain no prismatic or revolute joints. The

authors of [3] returned to Hirose’s concept by choosing a curve – an arc of a circle –

which closely matches the kinematics of continuum robots due to the equal distribution

of forces inherent in the design of continuum robots [14], an assumption also made

throughout this work. However, the model in [3] contains three problems. It assumes an

inextensible backbone, does not correctly determine the orientation of the trunk tip due to

a simplifying assumption in the model, and is limited to the particular physical design (a

unique actuator arrangement) of hardware implemented by the authors of [3].

These approaches constrain the performance of continuum robot hardware to re-

duced classes of shapes and motions, which in turn has restricted their potential applica-

bility. The first step in recovering this performance is to relate the shape of a continuum

robot to actuator lengths required to maintain that shape. Controlling the shape of the

manipulator requires a kinematic model relating shape in terms of extension and bending

to actuator inputs. This section outlines a new kinematic model, tailored to three-tendon,

1

20

°

configuration manipulator hardware such as the OctArm and Air-Octor trunks. The

model is both new and novel compared to previous models developed in [29], [3], which

were tailored to four-tendon sections and therefore not applicable to the more general de-

sign in this paper. Likewise, this model extends the three-cable kinematics in both the

trimole sensor [9], Hirose’s Elastor cable kinematics [16], and various active endoscopes

[11], [12], [13] by determining trunk length

s

based on an assumption of constant curva-

ture.

8

Transformation from cable lengths to trunk curvature

Given three known cable lengths

1

l

,

2

l

, and

3

l

in a section of the design, the constant dis-

tance

d

from the center of the trunk to the location of the cables, and

n

, the number of

segments in this trunk section, the following analysis allows computation of the resulting

trunk length

s

, curvature

κ

Ⱐ慮搠摩牥捴楯渠潦畲癡瑵牥,

φ

猠楬汵獴牡瑥搠楮⁆楧u牥′⸱⸠±周攠

瑨牥攠捡≥汥猠慲攠慳獵le搠瑯楥煵a汬礠獰 慣敤牯畮搠瑨攠瑲畮欬数慲慴敤礠慮杬敳映

1

㈰ °

. In addition, the curvature in each section of the trunk is assumed to be uniform, so

that trunk sections can be modeled as arcs of varying-radii circles. This is typically

achieved in hardware by distributing bending forces equally along the trunk using pneu-

matic pressure or springs [14].

This document discusses computation of

κ

Ⱐ

φ

Ⱐ慮搠

s

in three steps. First, equa-

tions are presented which determine the distances

1

r

,

2

r

, and

3

r

related to the trunk’s curva-

ture. Next, from these distances the maximum curvature

κ

琠慮杬攠

φ

猠摥物癥搮†䙩湡汬=,=

慤摩瑩潮慬煵慴楯湳潭灵瑥⁴桥⁴牵湫敮杴栠

s

from

κ

⸠.周牯畧桯畴⁴桩猠慮慬祳楳Ⱐ瑨θ=

=

=

䙩杵牥′⸱†θ±畮欠癡物慢汥猠

s

,

φ

Ⱐ慮搠

κ

⸠.周攠癡物慢汥θ

φ

楶敳⁴桥→瑡瑩潮渠瑨攠

x

y plane.

9

problem of solving geometrical problems posed by the trunk is accomplished by analyz-

ing a set of 2-D slices of the problem which allow computation of the desired quantities.

Computation of

i

κ

, the per-cable curvatures

Consider one bent segment in a section of the trunk as illustrated by Figure 2.2.

This figure shows three cables, spaced at

1

20°

intervals around the trunk, connecting two

segments in a section of the trunk. The figure includes plane

H

which passes through the

point at which cable 1 connects to the trunk and is perpendicular to the other two cables.

This plane exists due to the design of the trunk, which places the three cables at fixed in-

tervals about the circumference of the trunk, insuring that all three cables parallel each

other. Thus, a plane containing these two parallel cables exists, which implies the exis-

tence of a place perpendicular to that plane. This second plane, illustrated in the figure,

can be moved to include any single arbitrary point, such as the attachment point of cable

Cable 1

Cable 2

Cable 3

2

h

3

h

Plane

H

Figure 2.2 Bent segment in a section of the trunk

10

1 to the trunk.

Next, examine Figure 2.3. The figure now includes an additional triangle, drawn

from the center of the circle to the attachment point of cable 1, and it shows the hypote-

nuses of all three triangles. As demonstrated in the following section, the curvature of the

trunk can be calculated from

c

h

, the height of the center of the circle, point

c

in Figure

2.4, above the plane

H

. Finding this value requires knowledge of

2

h

and

3

h

, the heights of

the attachment points of cables 2 and 3 above imaginary plane as shown in Figure 2.3.

Although not shown in Figure 2.2, a second plane exists which is perpendicular to cables

2 and 3 and which contains the attachment point of cable 1 at the bottom of the trunk

segment. The lengths

2

2

h

and

3

2

h

by which cables 2 and 3 exceed the length

1

l

of cable 1

are equally divided between heights measured from above the top plane and below the

bottom plane. Therefore, the total length of cables 2 and 3, denoted

2

l

and

3

l

, along the

entire section of an

n

-segment trunk section can therefore be expressed using

2

h

and

3

h

as

2

h

3

h

c

h

Plane

H

Figure 2.3 Top portion of bent trunk segment, showing heights measured above an

imaginary plane

11

2 1

2

2

l l

h

n n

=

+

and (1)

3

1

3

2

l

l

h

n n

=

+

. (2)

Therefore,

2 1

2

2

l l

h

n

−

=

and (3)

3 1

3

2

l l

h

n

−

=

. (4)

Note that because the imaginary plane passes through the trunk at the attachment point of

cable 1,

1

0

h

=

by definition.

To find

c

h

, examine Figure 2.4 which shows the trunk viewed looking down on its

top segment from above. Inscribed in the circle which forms the trunk is an equilateral

triangle formed by drawing lines between each of the three cable attachment points. A

radius of the circle d drawn to the cable 1 attachment point divides the interior angle of

this triangle into two equal

3

0

°

halves. Therefore,

cos(30 )

d

a

° =

and

cos(30 )

b

d

° =

, yield-

ing the relationships

2

3

d

a =

and

3

2

d

b

=

. The hypotenuse of the triangle containing side

2

h

and of the triangle containing side

3

h

as shown in Figure 2.3 must therefore be

2 3b d=

.

12

Rotating this plane

9

0

°

along the dotted line produces Figure 2.5, which allows

computation of the height of points 2m and

3

m shown in Figure 2.4 above the plane

shown in Figure 2.3. Based on the calculations above, the length of the hypotenuse of the

larger triangle is

2 3b d=

while the length of the shorter hypotenuse is

2

3

d

. Therefore,

the height of point 2m above the plane given

1

0

h

=

,

2 1

m

h

, can be found from the similar

triangle relation

2

2 1

3

2

3

m

h d

d

h

=

. Substituting and solving for

2 1m

h

gives

2 1

2 1

3

m

l l

h

n

−

=

. Using

the same process for point

3

m

,

3 1

3 1

3

m

l l

h

n

−

=

. To find

c

h

, note that point

c

lies on the center

Point 2m

d

30° 30°

2 3b d=

Point 3m

Cables 2 and 3 attachment points

Point c

3

2

d

b =

2

3

d

a =

Cable 1 attachment point

Plane

A

Figure 2.4 Top-down view of trunk

13

of the circle in Figure 2.4. Therefore, its height must be the average of the heights of

points

2m

and

3

m

, resulting in

3 2 1

2

6

c

l l l

h

n

+

−

=. (5)

Note that

c

h

may be negative at some orientations of the trunk, where

1 2 3

2l l l> +

. Though

unexpected, the following geometrical uses of

c

h

are correct in this case.

Based on the assumptions, curvature of the trunk segment is evenly distributed

along each of the

n

sections in the segment. Therefore, the curvature of each section

κ

=

牥±a楮猠捯湳瑡湴汯湧⁴桥散瑩潮⸠i周攠 捵牶慴畲攠潦散瑩潮猠摥晩湥搠慳c

=

1

r

φ

κ

=

(6)

where

r

φ

is the radius of a circle which fits the arc produced by the bending of the trunk

section. Therefore, each segment bends along the circumference of a circle of radius

2 1

2

2

l l

h

n

−

=

2m

h

2

3

d

3d

Point

2m

Cable 2

attachment point

Cable 1

attachment point

Figure 2.5 Height of points 1 and 2 above plane

14

1

r

φ

κ

=. Consider a virtual cable of length

c

l

n

as shown in the figure, attached to the center

of a cross section of the trunk and traveling from the top to the bottom of a segment in the

trunk. At some as-yet unknown angle

φ

⁴桥楴畡瑩潮桯睮渠 䙩杵牥′⸶硩獴猬⁷h敲攠

慮獯獣敬敳⁴物慮杬攠睩瑨楤敳映汥湧瑨a

1

r

φ

κ

=

and

c

l

n

subtends

β

敧牥敳映慮牣映愠

捩牣汥潲c敤礠瑨攠瑲畮欮†䍯湳楤敲楮朠愠灬慮攠

A

猠獨潷渠楮⁆楧畲 攠㈮㐠捯湴慩湩湧⁴桥H

捩牣汥⁷桩捨潲m猠瑨攠瑯瀠潦⁴桥⁴牵湫Ⱐ湯瑥⁴s 攠汥朠潦⁴桩猠瑲楡湧汥e 睨楣栠汩敳渠瑨楳w

灬慮攮†周楳敧慮攠Hi敷敤猠瑨攠癥捴erH

κ φ

∠

渠=→污爠捯潲摩la瑥猠楮⁰污≥攠

A

⁷桥牥H

瑨攠潲楧楮f⁴桥⁰污湥猠瑨攠捥H瑥rH⁴桥⁴潰H⁴桥⁴牵湫Ⱐ灯楮琠

c

in Figure 2.4. The

magnitude of corresponding vectors

1

κ

Ⱐ

2

κ

Ⱐ慮搠

3

κ

Ⱐ湯瑡瑥搠

ㄳ

κ

−

Ⱐ睨楣栠灡獳⁴桲潵杨⁴桥,

慴瑡捨ae湴⁰潩湴猠景爠捡扬敳‱Ⱐ ㈬湤″汬潷2 瑥牭楮慴楯渠潦=

κ φ

∠

⁶楡潯牤楮慴攠瑲慮猭

∞→牭慴楯湳e瑡楬敤渠瑨攠湥硴e捴楯渮n

θ→潭灵瑥p

1

κ

Ⱐ牯瑡瑥散潮搠灬慮攠

B

桯睮渠䙩杵牥′⸶⁷桩捨猠灥牰敮摩捵H

污爠瑯⁰污湥l

A

潮瑡楮楮朠

l

n

φ

and

c

l

n

about

c

l

n

from angle

φ

⁴漠慮湧l攠潦e

9

0°, the location

of cable 1, so that the plane now contains

1

l

and

c

l

n

. Figure 2.7 shows the contents of

plane

B

H瑥爠牯瑡瑩潮Ⱐ睨楣栠捯it慩asH 慮獯a捥汥猠瑲楡湧汥⁷楴c楤敳H

1

r

and

c

l

n

. Similar to

the reasoning in (1) and (2), the length

c

l

n

of this virtual cable can be divided into

1

l

n

, the

length of cable 1 in one trunk segment, and

c

h

, the height this virtual cable extends above

the plane anchored by

1

l

, so that

1

2

c

c

l

l

h

n n

=

+. (7)

15

Relating this to the smaller similar triangle in the figure yields

1

1

2

c

c

l

h

h

n

d r

+

=

. Substituting

in (5) gives

( )

1 2 3

1

3 2 1

2

d l l l

r

l l l

+ +

=

+ −

. Noting that

1

r

φ

κ

=

from (6) yields

( )

3 2 1

1

1 2 3

2l l l

d l l l

κ

+ −

=

+ +

.

A similar process allows computation of

2

κ

⸠⁉n獴敡搠潦敦楮楮朠

1

0

h =

, define

2

0

h

=

. Rewriting equations (1) and (2) under this new assumption gives

1 2

1

2

l l

h

n n

= + and

3

2

3

2

l

l

h

n n

= +. Likewise, equations (3) and (4) become

1 2

1

2

l l

h

n

−

= and

3 2

3

2

l l

h

n

−

=. Consid-

inner

l

n

φ

s

n

β

d

d

outer

l

n

φ

c

l

n

1

d

κ

−

1

κ

Plane

A

Plane

B

Figure 2.6 Geometry of bent trunk segment

16

ering a redrawing Figure 2.4 in which the cable 2 attachment point is at the top of the fig-

ure,

1

2 2

3

2

3

m

h

d

d

h

=

where

2 2m

h

is the height of point

2m

above the plane assuming

2

0h

=

, so

1 2

2 2

3

m

l l

h

n

−

=

. In the same way,

3 2

3 2

3

m

l l

h

n

−

=

. Under this new assumption, then,

3 1 2

2

6

c

l l l

h

n

+ −

=

. Considering a redrawn Figure 2.7 gives

2

2 2

c

c

l

l

h

n n

=

+

. Relating the

smaller triangle in the figure to the larger triangle yields

2

2

2

c

c

l

h

h

n

d r

+

=

. Substituting for

c

h

produces

(

)

1 2 3

2

3 1 2

2

d l l l

r

l l l

+ +

=

+ −

. Applying (6) again,

d

1

r

3 2 1

2

6

c

l l l

h

n

+ −

=

1

2

l

n

c

l

n

Figure 2.7 Determination of

i

r

17

( )

3 1 2

2

1 2 3

2

l l l

d l l l

κ

+

−

=

+

+

. (8)

Repeating the same process but defining

3

0h

=

instead likewise results in

( )

1 2 3

3

2 1 3

2

d l l l

r

l l l

+ +

=

+ −

, so

( )

2 1 3

3

1 2 3

2l l l

d l l l

κ

+

−

=

+

+

. (9)

Computation of

κ

湤=

φ

Ⱐ瑨攠捵牶慴畲攠慮搠慮杬攠潦畲癡瑵牥,

θ→潮癥牴⁴桥=

i

κ

per-cable curvatures into

φ

Ⱐ瑨攠慮杬攠慴⁷桩捨⁴桥⁴牵湫敮摳猠

楬汵i≥±a瑥搠楮⁆楧畲攠㈮ㄬ湤≥

κ

Ⱐ瑨攠捵牶慴畲攠慴湧汥,

φ

Ⱐ捯湳楤敲整敲,楮楮朠

φ

湤=

κ

=

扡獥搠潮潯牤楮慴攠瑲慮獦潲ba瑩 潮映癡汵敳畳琠摥物癥搠景爠

1

κ

Ⱐ

2

κ

Ⱐ慮搠

3

κ

⸠⁎潴攠瑨慴a

瑨攠灯獩瑩潮猠潦慢汥猠ㄬ′湤″渠瑨攠捩 牣畭晥牥湣攠潦⁴桥⁴牵湫⁷楴栠牥獰散琠瑯⁴桥∞

x

axis are

9

0°, 210° and 30

−

° respectively as shown in Figure 2.8. Viewing these curva-

tures in plane

A

猠愠癥捴潲ⁱ畡湴楴楥猠批潴H 湧⁴桥湧l攠牥污瑩癥⁴漠瑨eH

x

axis along

which each curvature extends yields

1 1

90

κ

κ

=

∠ °

,

2 2

210

κ

κ

=

∠ °

, and

3 3

30

κ

κ= ∠− °

. A

combination of any two of these three vectors specifies a unique location in 2-D space.

In addition, any two of these three vectors forms a complete basis for 2-D space with ba-

sis vectors

(

)

(

)

T

1

1 90 cos 90 sin 90i = ∠ ° = ° °

⎡ ⎤

⎣ ⎦

,

( )

(

)

T

2

1 210 cos 210 sin 210i

=

∠ ° = ° °

⎡

⎤

⎣

⎦

,

and

( )

(

)

T

3

1 30 cos 30 sin 30i = ∠− ° = − ° − °

⎡ ⎤

⎣ ⎦

in the standard basis.

18

Choosing

{

}

㈳

,i i as basis vectors, the matrix

B

relates this basis to a more conven-

ient, standard or thogonal basis of

[

]

θ

1 0

x

i = ∠ ° = and

[

]

T

1 90 0 1

y

i = ∠ ° = according to

the equation

[

]

㈳

x y

i i i i

⎡ ⎤

=

⎣ ⎦

B

. Substituting for

2

i

,

3

i

,

x

i

, and

y

i yields

( ) ( )

( ) ( )

cos 210 sin 210

cos 30 sin 30

° °

⎡ ⎤

=

⎢ ⎥

− ° − °

⎣ ⎦

B

, so

1

3 3

3 3

1 1

−

⎡

⎤

−

⎢

⎥

=

⎢

⎥

−

−

⎢

⎥

⎣

⎦

B

. Note also that

1

y

i i=. Given

1

−

B

,

any vector given in the

{

}

㈳

,i i

basis can be transformed to the

{

}

,

x y

i i basis. For example,

the vector

(

)

2 3

,

κ κ

in the

{

}

㈳

,i i

basis is related to the vector

(

)

,

x y

κ

κ in the

{

}

,

x y

i i basis by

[

]

1

2 3

x y

κ κ κ κ

−

⎡ ⎤

=

⎣ ⎦

B

. Solving for scalars

x

κ

and

y

κ

in terms of known curvatures

1

κ

Ⱐ

2

κ

Ⱐ慮搠

3

κ

⁹楥汤猠

㌲

3

x

κ κ

κ

−

=

and

2 3 1y

κ

κ κ κ

=

− − =. In polar form,

1

tan

y

x

κ

φ

κ

−

⎛ ⎞

=

⎜ ⎟

⎝ ⎠

and

2 2

x y

κ

κ κ= +

. Substituting (8) and (9) then simplifying yields

( )

2 2 2

1 2 3 1 2 2 3 1 3

1 2 3

2

l l l l l l l l l

d l l l

κ

+

+ − − −

=

+ +

and (10)

1 y

i i=

3

i

2

i

x

i

Figure 2.8 Transformation of basis from the set

{

}

ㄲ 3

Ⱜi i i to the set

{

}

,

x y

i i.

19

1

3 2 1

2 3

2

3

tan

3

l l l

l l

φ

−

⎛ ⎞

+ −

=

⎜ ⎟

⎜ ⎟

−

⎝ ⎠

. (11)

Note that the numerator and denominator of the simplified expression for

φ

=瑡楮⁴桥=

獡se楧渠潦⁴桥物杩湡氠數灲敳獩潮Ⱐ= 汬潷楮朠畳攠潦畮捴楯湳業楬慲⁴漠䎒猠

atan2

rou-

tine which correctly determines

φ

牯洠

π

−

⁴漠

π

慴=敲⁴桡渠牥=≥物捴楮朠瑨±畴=u琠慮杬攠瑯=

瑨攠牡湧攠

,

㈲

π

π

⎛ ⎞

−

⎜ ⎟

⎝ ⎠

.

Computation of

s

, the trunk section’s length

Each section of the trunk is divided into

n

segments. In each segment, the cable

runs in a straight line from one cable guide to the next as shown in Figure 2.9. Solving

for the cable length in one section allows determination of the cable length in all sections.

Therefore, consider just one segment of the trunk. The length of this segment when un-

bent is therefore

s

n

.

Bending the trunk produces the geometry shown in Figure 2.6. Consider a virtual

cable of length

c

l

as shown in the figure, attached to the center of a cross section of the

trunk and traveling from the top and bottom of a segment of the trunk. As derived in (7),

1

2

c c

l

l nh

= +

. Substituting from (5) yields

3 2 1

3

c

l l l

l

+

+

=. (12)

20

The cable length

c

l

n

required for this curvature is also defined by the length of a chord

covering an angle

β

映愠捩牣汥映牡摩畳=

1

κ

Ⱐ慳桯睮渠瑨攠晩杵牥⸠

䥮楲捬攠潦慤極猠 r, the angle

ϕ

畢瑥湤敤礠愠灯牴楯渠潦⁴桥楲捵=晥牥湣攠潦∞

汥湧瑨l

a

obeys

2 2

r

a

π

π

ϕ

=

. To compute

β

潲⁴桥畴敲楲捬攠獨潷渠楮⁆楧畲攠㈮㘬桯潳攠

1

r

κ

=,

s

a

n

=, and

ϕ β

=. Solving for

β

⁹楥汤猠

=

s

n

κ

β

=. (13)

Examination of the triangle drawn inside the outer circle of Figure 2.6 shown in

Figure 2.10 reveals that

B

ending occurs only

at cable guides

Figure 2.9 Cables bend only at cable guides

21

2

sin

1

2

c

l

n

β

κ

⎛ ⎞

=

⎜ ⎟

⎝ ⎠

. (14)

Therefore, the overall cable length for this segment of the trunk is

2

sin

2

c

n s

l

n

κ

κ

⎛ ⎞

=

⎜ ⎟

⎝ ⎠

. If the

trunk is bent outward, then

κ

⁷楬氠=ee条瑩g攮†䥮⁴桩猠捡e攬⁴桥敮杴栠潦⁴=攠捨潲搠e±潳猭

楮朠瑨攠牡摩畳映潵瑥爠捩牣汥i 獨潷渠楮⁆楧畲攠㈮㘠杩癥猠瑨攠 摥獩牥≤慢汥敮l瑨⸠≥θh攠牡e

摩畳映瑨攠潵瑥爠捩牣汥猠

1

κ

−

⸠⁎潴攠瑨慴散慵獥s

(

) ( )

sin sin

θ

θ

−

= −, the expression

2

sin

2

c

n s

l

n

κ

κ

⎛ ⎞

=

⎜ ⎟

⎝ ⎠

remains correct in this case. Solving for

s

produces

1

2

sin

2

c

l

n

s

n

κ

κ

−

⎛ ⎞

=

⎜ ⎟

⎝ ⎠

.

Substituting from (10) and (12) yields

s

in terms of cable lengths as

( )

2 2 2

1 2 3

1 2 3 1 2 2 3 1 3

1

2 2 2

1 2 3 1 2 2 3 1 3

sin

3

nd l l l

l l l l l l l l l

s

nd

l l l l l l l l l

−

⎛ ⎞

+ +

+ + − − −

⎜ ⎟

=

⎜ ⎟

+ + − − −

⎝ ⎠

. (15)

2

β

2

c

l

n

1

κ

䙩杵牥′⸱〠⁃潭灵瑩湧p

s

.

22

Note in the limiting case as

2 2 2

1 2 3 1 2 2 3 1 3

0

g l l l l l l l l l

=

+ + − − − →,

1 2 3

0

l

im

3

g

l l l

s

→

+ +

= be-

cause

0

l

imsin

x

x x

→

=. Therefore,

1

0

l

im sin

x

x

x

−

→

=

so the limit for the entire expression be-

comes

( )

( )

2 2 2

1 2 3 1 2 2 3 1 31 2 3

1

2 2 2

0

1 2 3 1 2 2 3 1 3

2 2 2

1 2 3 1 2 2 3 1 31 2 3

1 2 3

2 2 2

1 2 3 1 2 2 3 1 3

l

im sin

3

.

3 3

g

l l l l l l l l l

nd l l l

nd

l l l l l l l l l

l l l l l l l l l

nd l l l

l l l

nd

l l l l l l l l l

−

→

⎛ ⎞

+ + − − −

+ +

⎜ ⎟

=

⎜ ⎟

+ + − − −

⎝ ⎠

+ + − − −

+ +

+ +

⋅ =

+ + − − −

(16)

In the case of a pneumatic robot, the pneumatic actuators can be considered to be cables

where the number of cable guides

n

→

∞

. Examining the limiting behavior of (15) in this

case shows that

1 2 3

lim

3

n

l l l

s

→∞

+

+

= (17)

by the same reasoning as (16), since all other terms cancel.

Transformation from trunk curvature to cable lengths

Consider the manipulator section shown in Figure 2.1. Given the length

s

of one section

of the manipulator,

κ

Ⱐ瑨攠捵牶慴畲攠潦⁴桡e 散瑩潮Ⱐ瑨攠慮杬攠

φ

琠睨楣=⁴桥=牶慴±±攠

汩敳l⁴桥楳瑡湣攠 d from the center of the manipulator to the location of the cables, and

the number of segments in this manipulator section

n

, this section discusses the computa-

tion of the lengths of the three cables

1

l

,

2

l

, and

3

l

, due to the shape of this section of the

trunk. See Figure 2.1. This is the inverse kinematics problem. Note that additional

length due to the configuration of proximal segments also affects the overall cable length.

23

Due to the difficulties of simultaneously solving three nonlinear equations, the inverse

kinematics will be derived from geometry underlying the trunk, rather than from the

equations for the forward kinematics.

As shown in Figure 2.11, the top of one segment in a section of the elephant trunk

lines in plane

A

⸠⁈敩杨瑳慮攠.敡獵牥搠晲潭散潮搠灬慮攠

C

which is parallel to

plane

H

shown in Figure 2.2 and in Figure 2.3. These two planes intersect along a single

line tangent to height h

φ

as shown in the figure. Viewing these two planes end-on as in

Figure 2.12, note that heights measured from plane

C

to a point on

A

漠湯琠捨慮来⁷桥渠

景汬潷楮朠慮礠汩湥渠

A

⁰慲慬汥氠瑯⁴桩猠瑡湧敮琮†䙯爠 數慭灬攬猠楬汵獴牡瑥搠楮⁆楧畲攠

㈮ㄴⰠ灲潪散瑩湧⁴桥敩杨琠

1

h

along a line parallel to the tangent at h

φ

gives

1 1p

h h=, where

1p

h is the height at a point along the line connecting

c

h

and h

φ

. Knowledge of the slope the

line created by viewing plane

A

湤ⵯn湤H⁰潳楴楯H渠瑨楳楮攠i汬潷s潭灵瑡瑩潮p

䡥楧桴h

h

φ

at angle

φ

.

䑥晩湥搠慳α

0h

φ

=

Cable 1, height is

1

h

Cable 2, height is

2

h

Cable 3, height is

3

h

Trunk center, height is

c

h

Line at which

planes intersect

Plane

A

H

Pl慮攠

H

&

C

to plane

H

Figure 2.11 Plane containing circle and reference plane

24

of the height of this position above plane

C

, which appears as horizontal line in the figure.

Therefore, the height of an arbitrary point on the circle such as

1 1p

h h

=

can be determined

by projecting the point along a line parallel to the tangent at

h

φ

onto the diagonal line in

Figure 2.12 then applying the slope-intercept form of that line to find the point’s height.

In the forward kinematics derivation, the height

1

0

h

=

was chosen to anchor the

plane shown in Figure 2.2. For the inverse kinematics derivation, the plane will be an-

chored by 0

h

φ

= instead. Examining the isosceles triangle shown in Figure 2.6 with sides

1

κ

and

c

l

n

creates Figure 2.13 which reveals

sin

2

c

h

d

β

⎛ ⎞

=

⎜⎟

⎝⎠

. Solving for

c

h

and substituting

(13) yields

sin

2

c

s

h d

n

κ

⎛

⎞

=

⎜

⎟

⎝

⎠

. Projecting these points onto the diagonal line of Figure 2.12

allows determination of the slope and intercept of the line

y mx b

=

+ based on the known

values

c

h h

m

d

φ

−

=

,

x

d a

= − and

b

h

φ

=

which yields

( )

1 1

c

p

h h

h h d a h

d

φ

φ

−

=

= − +

as

shown in Figure 2.15. This projection is also illustrated in Figure 2.14, where projecting

Cable 2

Cable 3

Center

Point at

φ

Ⱐ

睨敲攠

0

h

φ

=

Cable 1

2

h

3

h

c

h

1

h

d

a

Plane

A

癩敷敤e

敮搭潮e

Pl慮攠

C

viewed

end-on

Figure 2.12 Rotated view of Figure 2.11

25

the height

1

h

along a line parallel to the tangent gives

1 1p

h h

=

, where

1p

h

is the height at a

point along the line connecting

c

h

and

h

φ

as shown in the figure. Also in this figure, note

that

( )

cos 90 sin

a

d

φ φ

° − = =. Substituting and simplifying,

( )

1

sin 1 sin

2

s

h d

n

κ

φ

⎛ ⎞

= −

⎜ ⎟

⎝ ⎠

.

Similar to the reasoning in (3) and (4), the length

1

l

of cable 1 can be divided into

l

φ

Ⱐ瑨攠汥湧瑨映愠癩牴畡氠 捡扬攠慴瑡捨敤琠慮杬攠

φ

渠瑨攠捩牣畭晥牥湣攠潦⁴桥a湩灵污n

瑯爠慮搠牵湮楮朠瑨攠汥 湧瑨映瑨攠na湩灵污瑯nⰠ慮搠瑨攠桥楧桴,

1

h

of cable 1 above the plane

anchored by

l

φ

Ⱐ獯⁴桡琠

ㄱ

2

l

l nh

φ

= +. To find

l

φ

Ⱐ湯瑥⁴桡琠瑨攠汥湧瑨映愠捡扬攠

l

φ

瑴慣桥搠

瑯⁴桥楲捵浦敲敮捥映瑨攠瑲畮欠慴 ⁴桥⁰潩湴映=a硩xu洠←u牶慴畲攠

φ

→牭猠瑨攠瑲楡sg汥l

獨潷渠瑨攠楮湥爠捩牣汥映䙩杵牥′⸶Ⱐ牥摲慷 渠楮⁆楧畲攠㈮ㄶⰠ祩敬摩 湧敬慴楯湳桩瀠獩ni-

=

2

β

c

h

d

β

c

l

n

Figure 2.13 Computing

c

h

from

β

=

=

26

lar to (14),

2

sin

1

2

l

n

d

φ

β

κ

⎛ ⎞

=

⎜ ⎟

⎝ ⎠

−

. Therefore, substituting (13), the cable length can be com-

puted as

1

2 sin

2

s

l

n d

n

φ

κ

κ

⎛ ⎞ ⎛ ⎞

= −

⎜ ⎟ ⎜ ⎟

⎝ ⎠ ⎝ ⎠

. Substituting and simplifying,

1

1

2 sin sin

2

s

l

n d

n

κ

φ

κ

⎛ ⎞⎛ ⎞

= −

⎜ ⎟⎜ ⎟

⎝ ⎠⎝ ⎠

. (18)

Likewise, rotating to 210° and

3

30° to find

2

h

and

3

h

, the heights of the attachment points

of cables 2 and 3, then re-solving produces

( )

2

1

2 sin sin 60

2

s

l

n d

n

κ

φ

κ

⎛ ⎞⎛ ⎞

= + ° +

⎜ ⎟⎜ ⎟

⎝ ⎠⎝ ⎠

and (19)

a

φ

9

0

φ

° −

φ

d

Plane

A

H

1

h

1p

h

c

h

h

φ

≤

Figure 2.14 Computing

a

in Figure 2.12

27

( )

3

1

2 sin cos 30

2

s

l

n d

n

κ

φ

κ

⎛ ⎞⎛

⎞

= − ° +

⎜ ⎟⎜

⎟

⎝⎠⎝

⎠

. (20)

Note that because

0

l

imsin

x

x x

→

=, in the limiting case

1

0

1

l

im 2 sin

2

s

l n d s

n

κ

κ

φ

κ

→

⎛ ⎞⎛ ⎞

=

− =

⎜ ⎟⎜ ⎟

⎝ ⎠⎝ ⎠

. In

the same way,

2 3

0 0

l

im lim

l l s

κ

κ

→ →

= =. In the case of a pneumatic manipulator, where

n

→

∞

,

these equations become

( )

1

2

3

1 sin

1 sin

3

1 cos

6

s d

l

l s d

l

s d

κ φ

π

κ φ

π

κ φ

⎡

⎤

⎢

⎥

−

⎢

⎥

⎡ ⎤

⎢

⎥

⎛ ⎞

⎛ ⎞

⎢ ⎥

= + +

⎢

⎥

⎜ ⎟

⎜ ⎟

⎢ ⎥

⎝ ⎠

⎝ ⎠

⎢

⎥

⎢ ⎥

⎣ ⎦

⎢

⎥

⎛ ⎞

⎛ ⎞

⎢

⎥

− +

⎜ ⎟

⎜ ⎟

⎢

⎥

⎝ ⎠

⎝ ⎠

⎣

⎦

, (21)

which were derived by examining lim

n

→∞

of (18), (19), and (20).

Summary

This section discusses the derivation of the following equations which convert

known cable lengths into trunk curvature. From equations (10), (11), (15), (16), and (17),

c

h

h

φ

≤

Figure 2.15 Line used to derive slope and intercept.

28

( )

2 2 2

1 2 3 1 2 2 3 1 3

1 2 3

2

l l l l l l l l l

d l l l

κ

+

+ − − −

=

+ +

and (22)

1

3 2 1

2 3

2

3

tan

3

l l l

l l

φ

−

⎛ ⎞

+ −

=

⎜ ⎟

⎜ ⎟

−

⎝ ⎠

. (23)

( )

2 2 2

1 2 3

1 2 3 1 2 2 3 1 3

1

2 2 2

1 2 3 1 2 2 3 1 3

sin

3

nd l l l

l l l l l l l l l

s

nd

l l l l l l l l l

−

⎛ ⎞

+ +

+ + − − −

⎜ ⎟

=

⎜ ⎟

+ + − − −

⎝ ⎠

(24)

where

2

1

1 2 3

0

lim

3

l

l l l

s

+ →

+ +

=

…

and

1 2 3

3

l l l

s

+

+

= (25)

in the case of a pneumatic manipulator. It also discusses the following equations which

convert trunk curvature into cable length. From (18), (19), (20), and (21),

2

β

2

l

n

φ

1

d

κ

−

Figure 2.16 Determining cable length

29

( )

( )

1

2

3

1

2 sin sin

2

1

2 sin sin 60

2

1

2 sin cos 30

2

s

n d

n

l

s

l n d

n

l

s

n d

n

κ

φ

κ

κ

φ

κ

κ

φ

κ

⎡ ⎤

⎛ ⎞⎛ ⎞

−

⎜ ⎟⎜ ⎟

⎢

⎥

⎝ ⎠⎝ ⎠

⎢

⎥

⎡ ⎤

⎢

⎥

⎛ ⎞⎛ ⎞

⎢ ⎥

= + ° +

⎢

⎥

⎜ ⎟⎜ ⎟

⎢ ⎥

⎝ ⎠⎝ ⎠

⎢

⎥

⎢ ⎥

⎣ ⎦

⎢

⎥

⎛ ⎞⎛ ⎞

− ° +

⎢

⎥

⎜ ⎟⎜ ⎟

⎝ ⎠⎝ ⎠

⎣ ⎦

(26)

where

1 2 3

0 0 0

l

im lim liml l l s

κ

κ κ

→ → →

= = =. In the case of a pneumatic manipulator,

( )

1

2

3

1 sin

1 sin

3

1 cos

6

s d

l

l s d

l

s d

κ φ

π

κ φ

π

κ φ

⎡

⎤

⎢

⎥

−

⎢

⎥

⎡ ⎤

⎢

⎥

⎛ ⎞

⎛ ⎞

⎢ ⎥

= + +

⎢

⎥

⎜ ⎟

⎜ ⎟

⎢ ⎥

⎝ ⎠

⎝ ⎠

⎢

⎥

⎢ ⎥

⎣ ⎦

⎢

⎥

⎛ ⎞

⎛ ⎞

⎢

⎥

− +

⎜ ⎟

⎜ ⎟

⎢

⎥

⎝ ⎠

⎝ ⎠

⎣

⎦

. (27)

CHAPTER 3

TRUNK KINEMATICS

As discussed in the introduction and previous chapter, efforts in modeling and real-time

algorithm development to extract the full physical potential from continuum robots have

lagged behind. For example, almost all previous approaches either do not address trunk

orientation, approximate positional kinematics, or solve them only for limited kinematic

models, which constrain the performance of continuum robot hardware to reduced classes

of shapes and motions, restricting their potential applicability. In this chapter, we modify

and extend some of the ideas in [3] as part of a new and quite general kinematic formula-

tion for continuum robots. This approach, unlike previous work, produces complete and

correct results for both task-space position and orientation. The method fully models for

the first time, to the best of our knowledge, the exact kinematics of trunk sections which

can extend in length as well as bend in two dimensions using closed-form equations. The

approach is also modular, allowing its application to a wide range of physical realiza-

tions, including in particular the common three evenly-spaced tendons per section design.

In addition, the resulting Jacobians, derived via a straightforward application of well-

known D-H techniques and the chain rule, are relatively simple and computable in real

time. This model also allows a straightforward analysis of singularities present in a con-

tinuum manipulator. In the following sections, we discuss the modeling approach, de-

scribe the various kinematic transformations and Jacobian synthesis in detail, analyze

31

uniqueness, singularities, and accuracy, and report results from a real-time hardware im-

plementation on two different multi-section continuum robots.

This chapter focuses on extending, correcting, and improving a method first de-

veloped by Hannan [3] to derive positional and velocity kinematics for trunk-like ma-

nipulators. Hannan’s method utilized transformations which relate the forward kinemat-

ics derived from a Denavit-Hartenburg (D-H) table to trunk positional kinematics. He

then implemented positional (but not orientational) kinematics, enabling control of the

robot via a joystick. While the position of the trunk’s end effector obtained and imple-

mented was correct, the orientation of the end effector resulting from this table, which

was not implemented, is incorrect. Two revised D-H tables with associated transforma-

tions presented here correct this error in the orientational term. Hannan’s method for de-

riving linear velocity kinematics departed from traditional D-H techniques, instead deriv-

ing linear velocity kinematics by taking a series of derivates of the positional kinematics.

This chapter develops methods to apply D-H techniques to obtain velocity kinematics,

yielding linear velocity kinematics which are easier to compute. These techniques also

produce angular velocity kinematics, unlike Hannan’s method, which relied on hand-

derived geometrical methods to incorrectly determine angular velocity kinematics for the

trunk. Finally, the modular nature of this approach allows robot-specific portions of the

derivation to be easily exchanged in order to derive equations for a given robot.

Trunk modeling and mechanics

The method pioneered by Hannan is but one of many choices. Unlike rigid-link

robots which rely the well-established Denavit-Hartenburg (D-H) procedure to derive po-

sitional and velocity kinematics and the Euler-Lagrange equations to derive dynamics,

32

the development of kinematics and dynamics for trunk-like robotic manipulators can fol-

low several paths. Gravagne [14], [29] applied techniques from mechanics to derive a set

of partial differential equations which, when solved, yields both kinematics and dynamics

for a trunk. While these equations result in solutions for a 2-dimensional case, solutions

for this approach in 3 dimensions have not been found. A second approach is that fol-

lowed by Selig [30], in which techniques from differential geometry applied to Cossert

rod theory provided expressions for trunk dynamics as transcendental functions which

require numerical solution. Chirikjian and Burdick [25] applied a modal decomposition

to approximate the shape of a continuum robot with an arbitrary curve. Finally, Hannan

[31] used techniques based on classical calculus, Euclidian geometry, and D-H tables to

derive the kinematics for a trunk.

Of these techniques, the most promising for the derivation of kinematics for the

real-time control of a trunk robot is the method developed by Hannan then extended in

this chapter. Most importantly, these techniques allow the derivation of positional and

velocity kinematics in three dimensions in closed-form equations, unlike many tech-

niques. In addition, they make use of straightforward plane geometry and of a well-

established and understood body of work, the Denavit-Hartenburg procedure, resulting in

solutions which can be more easily understood, implemented, and extended. Finally,

Figure 3.1 and Figure 3.2 illustrate two robotic trunks which obey the underlying con-

stant-curvature assumption made by this method, providing an important practical appli-

cation for this theory.

33

Deriving kinematics using this method involves two steps. First, this work ex-

tends Hannan’s method by formulating the trunk kinematics problem as a series of substi-

tutions applied to a homogenous transformation matrix computed using a Denvait-

Hartenburg (D-H) type approach. As in Hannan’s method, this formulation relies on the

assumption that the trunk bends with constant curvature, due to the compliance of the

trunk [14]. Second, velocity kinematics are developed by first determining the Jacobian

using standard techniques based on the D-H table then by chaining together the Jacobians

produced by computing derivates of each succeeding substitution. The following para-

graphs outline this approach, while the next section develops the resulting equations in

detail.

Figure 3.1 The Air-Octor two-section continuum robot.

34

In conventional manipulator forward kinematics, a standard homogenous trans-

formation matrix

( )

,d

θ

A

produced via a D-H table is used to transform local shape coor-

dinates

(

)

,d

θ

into task coordinates

x

, representing both end effector position and orienta-

tion. This is based on a series of independent rotations

i

θ

and translations

i

d

with one in-

dependent variable per joint at each of the

i

joints. However, continuum robots lack ex-

plicit joints, with the equivalent to rotations

i

θ

and translations

i

d

being an arc of constant

curvature modeled by trunk parameters

s

,

κ

Ⱐ慮搠

φ

⁷桥牥=

s

gives trunk length,

κ

整敲=

←i湥猠捵牶慴畲攬湤n

φ

整敲=楮敳⁴桥湧汥=畲癡瑵=攠慳桯睮渠䙩杵牥′⸱⸠A物瑩=

=

=

䙩杵牥″⸲†周攠佣瑁牭潵θ散瑩潮潮瑩湵畭潢潴⸠

=

35

cal question for continuum robot kinematics is how to modify conventional robot kine-

matics to fit this case.

A fundamental concept exploited in this chapter, first introduced by Hannan, is to

fit a conceptual conventional rigid-link manipulator to the continuous backbone of a con-

tinuum robot. Using simple geometric arguments, Hannan’s work determined the rela-

tionship

[

]

( )

T

1

,,d f s

θ

κφ= between D-H table parameters

i

θ

and

i

d

and the trunk pa-

rameters

s

,

κ

Ⱐ慮搠

φ

⸠䥭灬楣楴畢p瑩瑵瑩≥n湡扬敳=浰畴慴楯渠←∞⁴牵湫⁴楰⁰→獩瑩潮湤=

潲楥湴慴楯渠

x

as a function of

s

,

κ

Ⱐ慮搠

φ

猠

(

)

(

)

(

)

1

,,,,f s s

κ

φ κφ

=

A A

. This allows the

use of modified D-H procedures to relate end effector task coordinates to robot shape [3].

Next, to map

s

,

κ

Ⱐ慮搠

φ

⁴漠獰散楦楣潮瑩湵畭潢潴= 桡牤睡牥Ⱐ慮潴桥爠瑲慮獦潲h

←a瑩潮猠湥捥獳慲礠睨楣栠癡物敳慳敤渠瑨 攠捯湳瑲畣瑩潮映瑨攠牯扯琬敭潮獴牡瑩湧→

瑨攠≥潤畬慲楴礠潦⁴桩猠慰灲→ 慣栮⁆潲⁴桥⁃l敭s潮⁅汥灨慮琠θ±畮欠ua湩灵污瑯pⰠ瑨攠睯牫,

楮⁛㍝整敲i楮敤i灰楮朠

(

)

(

)

2

,,

a

s k f l

φ

=

to compute

s

,

κ

Ⱐ慮搠

φ

楶敮⁴桥敮杴栠

l

of the four cables which controlled each trunk section. That work, while innovative, was

limited to the unique actuator arrangement in the robot in [3], and assumed constant trunk

length, which restricted the generality of the results. In this paper, we introduce an analo-

gous mapping of

(

)

(

)

2

,,

b

s k f l

φ

=

from the lengths

l of each of the three cables per sec-

tion to trunk parameters

s

,

κ

Ⱐ慮搠

φ

汬=獴牡瑥搠楮⁆楧u牥′⸱→爠±污獳=敳i杮猠瑹灩gi敤e

批⁴桥⁃汥←獯渠䅩A-佃OO删牯扯琠獨潷渠楮⁆楧R 牥″⸱⸠周楳⁰慲瑩捵污爠慣瑵慴潲牲慮来θ

←e湴猠煵楴攠捯浭潮捲潳猠愠睩摥⁶慲楡瑩潮n 楮潮瑩湵畭潢潴慲摷慲攠牥慬楺慴楯湳=

獵捨猠嬱ㅝⰠ嬲㉝Ⱐ嬱㝝Ⱐ 嬲[]Ⱐ嬲㍝Ⱐ嬱,],⁛ㄳ崮†䅬 瑥牮≥瑩癥ly,⁴桩牤a灰楮朠

(

)

( )

2

,,

c

s f pκφ = between the air pressure in each of the three McKibben actuators in a

36

section of the OctArm manipulator [6] shown in Figure 3.2 may be utilized. Therefore,

this work is more generally applicable than the results in [3].

In summary, these forward kinematic transformations relate trunk tip position and

orientation

x

to the cable length

l or pressure

p

of the robot according to the information

flow

1 2

,,,/

f f

D H

x

d s l p

θ

κφ

−

←⎯⎯⎯ ←⎯ ⎯ ←⎯ ⎯

(28)

or, more formally,

(

)

(

)

(

)

1 2

/

D H

x

f f f l p

−

=

. (29)

The corresponding transformations are fully developed and analyzed in the next section.

In forward velocity kinematics, the goal in the context of continuum robots is to

relate trunk tip linear and angular velocity

x

to either cable velocities

l

or actuator pres-

sures

p

through a Jacobian, so that

x

l

=

J

or

x

p

=

J

. Following the approach of (28), this

can be accomplished through a series of Jacobians according to the information flow

(

)

(

)

(

)

1 2

,,,/

f f

D H

x

d s l pθ κφ

−

←⎯⎯⎯ ⎯ ←⎯⎯ ⎯ ←⎯⎯ ⎯

J J

J

. (30)

That is, a straightforward application of the chain rule to (29) yields

( )

(

)

( )

(

)

( )

(

)

( )

1 2

/

,,,

or

,,,

/

=/,

D H f f

d l p

d s

dx x

dt d s dt

l p

x l p

θ κφ

θ κφ

−

∂ ∂

∂

=

∂ ∂

∂

J J J

(31)

37

where

1 2

D

H f f−

=

J

J J J. This formulation reveals an advantage of this approach: different

Jacobian “modules” such as

2

a

f

J

for the Clemson Elephant Trunk,

2

b

f

J

for Air-OCTOR, or

2

c

f

J

for OctArm allow different hardware realizations across a range of continuum hard-

ware designs to be easily plugged in to the derivation.

The use of standard techniques produces the Jacobian

D

H

−

J

from an appropriate

D-H formulation, while

1

f

J

and

2

f

J

are directly computed as partial derivatives of

1

f

and

2

f

. Before computing these equations, the underlying assumptions made by this model

must be validated by actual robots for the equations to find application. First, this model

assumed that trunks bend with constant curvature. That is, each section of the trunk can

be modeled as an arc of a circle of some fixed radius r and curvature

1

r

κ

=, as opposed to

trunk sections which bend with curvature

(

)

s

κ

where curvature varies along the length

s

of the trunk section. Second, the radius of each trunk section is assumed to be constant

and any cable guides are assumed to be equally spaced, though both the radius and num-

ber of cable guides can vary from section to section. Finally, these trunks do not exhibit

torsion, defined as twisting along the length of the trunk.

Having reviewed a general approach for determining kinematics and validating

the assumptions made by the model on actual devices, the next section will then develop

equations for positional kinematics, followed by a section on velocity kinematics.

Positional Kinematics

As discussed in the introduction to this chapter, the DH table developed by Hannan [31]

produces correct positional information but incorrect orientational information. One ap-

proach to correcting the error in the orientation term is to derive a new D-H table which

38

more accurately models the physical construction of the robot. A second approach is to

add an additional term to the D-H table derived by Hannan to correct the orientational

error. The following sections detail results obtained from following both approaches.

Modified DH table

These paragraphs will first review the D-H table and geometrical transformations devel-

oped by Hannan, discuss the orientation problem introduced by trunk torsion designed

into the first entry of the existing D-H table, then introduces an anti-torsional term to the

end of table which cancels out the torsion introduced earlier, thus solving the problem.

The following section, in contrast, develops a new D-H table which solves the problem

without resorting to any torsional terms. Comparisons between the two then reveal that

both approaches yield identical results.

Hannan’s approach simplifies the problem of finding the positional kinematics

which relate trunk tip position and orientation

x

to trunk parameters

s

,

k

, and

φ

猠楬汵猭

瑲慴敤礠瑨攠晩牳琠慲牯眠楮
㈸⤠批楲獴潬癩湧⁴桥⁰牯扬2 ←渠瑷漠摩浥湳楯湳⁴桥渠數=

瑥湤楮朠瑨攠獯汵瑩潮⁴漠瑨牥攠摩≥e湳楯湳⸠n 䙯汬潷楮朠桩猠浥瑨潤猬 潮獩摥爠瑨敲敦潲攠愠

瑲畮欠獥捴楯渠楮′ⵄ⁷楴栠橯楮瑳 猠楬汵獴牡瑥搠楮⁆楧畲攠㌮㌠ 捯牲敳灯湤楮朠瑯⁴桥⁄ⵈ⁴慢汥c

楮iθa扬攠㌮b.†乯瑥⁴=慴楮欠ㄬ⁷桩a栠睩汬h 瑥≥≤⁴桥獥煵=瑩潮猠瑯″ⵄ,猠摥汩扥±慴敬a=

潭楴瑥搮†周攠ei牳琠牥癯汵瑥±楮琠灯楮瑳⁴桥i 捡氠捯潲摩湡瑥祳瑥≥⁴潷慲搠瑨攠瑩瀠潦⁴桥=

瑲畮欠睨楣栠扥湤猠楮⁴桥≥

x

−

楲散瑩潮Ⱐ灥爠䡡湮慮=猠捯湶敮 瑩潮⸠≥周攠獥捯湤潩湴Ⱐ睨楣栠

楳⁰物獭a瑩挬⁴牡湳污瑥猠瑨攠汯捡氠=±a←攠瑯⁴e 攠瑩瀠瑨敮⁴桥i湡氠牥癯汵≥e潩湴物敮瑳⁴桥=

捯潲摩湡瑥祳瑥c汯湧⁴桥⁴慮来湴⁴漠瑨 攠瑲畮殒猠瑩瀮†周楳⁴慢汥⁰牯摵捥猠瑨攠桯θ潧攭

湯畳⁴牡湳景牭a瑩潮a瑲楸≥

39

(

)

(

)

( ) ( )

2 4 2 4 3 1

2 4 2 4 3 1

cos sin 0 sin

sin cos 0 cos

0 0 1 0

0 0 0 1

d

d

θ

θ θ θ θ

θ

θ θ θ θ

⎡ + − + − ⎤

⎢ ⎥

+ +

⎢ ⎥

=

⎢ ⎥

⎢ ⎥

⎣ ⎦

A

, (32)

also found as equation 3.40 in Hannan [31].

Next, given this D-H table and resulting

A

matrix, Hannan develops a function to

transform D-H parameters

θ

湤=

d

into trunk parameters

s

,

κ

Ⱐ慮搠

φ

Ⱐ湯瑡瑥搠慳,

1

f

in (28).

The trunk segment shown in Fi gure 3.4, an arc of length

s

which subtends

β

慤i慮a映==

捩牣汥映牡li畳u r, contains an isosceles triangle with two sides of length

1

r

κ

=

. Subdivid-

ing this triangle, note the relationship

2

sin

1

2

d

β

κ

=

. Therefore,

2

sin

2

d

β

κ

=

. For any arc of

a circle

2 2

s

r

β

π

π

=

so that s

β

κ=

where

1

r

κ

=

. Substituting,

2

sin

2

s

d

κ

κ

=

, (33)

2

θ

4

θ

3

d

x

−

y

Figure 3.3 D-H joints in Hannan’s 2-D table. Revolute joints are marked by circles and

prismatic joints by boxes.

40

which can also be derived by substituting (11) into (12) in [32]. Note also the triangle

similarity which demonstrates

2

β

γ

=

Ⱐ獯⁴桡琠

2

s

κ

γ

=

. Examining the D-H table and ac-

companying illustration shows that

2 4

θ

θ γ

=

=

and

3

d d

=

. Therefore,

3

2

sin

2

s

d

κ

κ

=

and

2 4

2

s

κ

θ

θ

= =

. We have now developed a transformation

1, 2-D

f

between D-H parameters

and trunk variables for the 2-D case:

[

]

( )

T

T

2 3 4 1,2-D

2

,sin

2 2 2

s s s

d f s

κ κ κ

θ θ κ

κ

⎡ ⎤

= =

⎢ ⎥

⎣ ⎦

.

Substituting this into (32),

cos 1

cos sin 0

sin

sin cos 0

0 0 1 0

0 0 0 1

s

s s

s

s s

κ

κ κ

κ

κ

κ κ

κ

−

⎡

⎤

−

⎢

⎥

⎢

⎥

⎢

⎥

=

⎢

⎥

⎢

⎥

⎢

⎥

⎢

⎥

⎣

⎦

A

, (34)

identical to Hannan [31] equation 3.40.

Extending this to three dimensions then proves straightforward. Adding a single

torsional joint to the beginning of 2-D D-H table which rotates the trunk out of the

x

y

plane enables the trunk to move in full

x

yz space. In addition, changing link 4’s

α

⁰愭

θa扬攠㌮ㄠ⁈慮湡溒猠䐭s⁴慢汥渠㈭=.†䅳瑥物 獫猠sa牫潩湴⁶慲楡±汥献†䱩湫‱Ⱐ睨楣栠數l

瑥湤猠瑨攠瑡el攠瑯″-䐬猠湯琠灲敳敮琮=

䱩湫

a

α

d

θ

=

㈠2

## Commentaires 0

Connectez-vous pour poster un commentaire