Kinematic Modelling of continuum robots following

constant curvature

Centre for Robotics Research – School of Natural and Mathematical Sciences – King’s College London

Hongbin Liu

Compliant Robotics Peking University, Globex, July 20182

• Continuum robots are increasing popular in modern robotics

• Continuous design can be highly advantageous for

– Compliant adaptation to unknown environments

– Safe manipulation of fragile objects and in human-robot interaction

Introduction to Constant Curvature Model

Compliant Robotics Peking University, Globex, July 20183

• Unlike rigid-linked, hyper-redundant robots no discrete links

• Continuous deformation of robot’s body allows for motions such as

– Bending around unknown objects for manipulation

– Dexterous movements for locomotion in uncertain terrain

Introduction to Constant Curvature Model

Compliant Robotics Peking University, Globex, July 20184

• Until now:

– Robot fully defined for a given set of joint angles and link lengths

Introduction to Constant Curvature Model

Compliant Robotics Peking University, Globex, July 20185

• Until now:

– Robot fully defined for a given set of joint angles and link lengths

• Now Continuum robots:

– Underactuated system; infinite dofs have to be addressed while only limited number of dofs can be controlled

– Forces and moments have to be considered due to inherent elastic behaviour

Introduction to Constant Curvature Model

Compliant Robotics Peking University, Globex, July 20186

• Until now:

– Robot fully defined for a given set of joint angles and link lengths

• Now Continuum robots :

– Underactuation of the system; infinite dofs have to be addressed while only limited number of dofs can be controlled

– Forces and moments have to be considered due to inherent elastic behaviour

Assumptions necessary to recreate shape of robot

Introduction to Constant Curvature Model

Compliant Robotics Peking University, Globex, July 20187

Definition of Constant Curvature

• Shape of robot can be recreated assuming a Constant Curvature

• The reason will be discussed in Mechanical Modelling

• CC allows for geometrical description of robot following a curve with curvature k, bending radius r, angle of bending plane φ and arch length l

Compliant Robotics Peking University, Globex, July 20188

• Model can be generated based on mappings between three spaces

– Actuator (tendon driven) space

– Configuration space

– Task space

• Each space comprises set of descriptive variables and spaces changed using mapping functions

Mapping

Compliant Robotics Peking University, Globex, July 20189

• Robot configuration can be expressed as a set of previously defined arc parameters (k, φ and l)

• Given these parameters the kinematic description can be achieved applying universal modelling techniques, independent from robot type

Mapping

Compliant Robotics Peking University, Globex, July 201810

• Robot configuration can be expressed as a set of previously defined arc parameters (k, φ and l)

• Given these parameters the kinematic description can be achieved applying universal modelling techniques, independent from robot type

• Mapping from actuator to configuration space is robot-specific

Mapping

Compliant Robotics Peking University, Globex, July 201811

Example Tendon driven:

Continuously bending actuators

• Tendon driven leading to length change

• Placement of actuators allows bending motions of robot

• Robot tip position and orientation can be described as a

function of the actuator lengths

Mapping: Actuator to Config. space

Compliant Robotics Peking University, Globex, July 201812

Example: Tendon driven

CMU snake robot

Compliant Robotics Peking University, Globex, July 201813

Example: Tendon driven

OC snake robot, UK

Compliant Robotics Peking University, Globex, July 201814

Example: Tendon driven

Wire-Driven Flexible Robot Arm

The china university of Hong Kong

Compliant Robotics Peking University, Globex, July 201815

Example: fluidic actuation:

Continuously bending actuators

• Inflatable chambers leading to length change upon pressurization

• Placement of actuators allows elongation and bending motions of robot

• Robot tip position and orientation can be described as a

function of the actuator lengths

Mapping: Actuator to Config. space

Compliant Robotics Peking University, Globex, July 201816

Example: fluidic actuation

Festo arm, Germany

Compliant Robotics Peking University, Globex, July 201817

Example: fluidic actuation

Stiff-Flop manipulatorEU FP7 King’s College London, UK

• Silicone-based soft body

• Multi-segment continuum robot with

3 dofs per element

• Fluidic actuation (air pressure or

hydraulic)

• Multi-purpose platform for minimally-

invasive surgery

Compliant Robotics Peking University, Globex, July 201818

Example: fluidic actuation

L1L3L2

Compliant Robotics Peking University, Globex, July 201819

Example: fluidic actuation

Compliant Robotics Peking University, Globex, July 201820

Constant curvature in 2D

𝑙1 = 𝜃𝑟𝑙2 = 𝜃 𝑟 + 2𝑑𝑙 = 𝜃(𝑟 + 𝑑)

𝑙2 = 𝜃 𝑟 + 2𝑑 + 𝜃𝑟 − 𝜃𝑟

𝑙2 = 2𝜃 𝑟 + 𝑑 − 𝜃𝑟𝑙2 = 2𝑙 − 𝑙1

𝑑𝑟

𝜃

𝑑

𝑙1

𝑙2

x

y

x1

y1

−𝜃

o1

o 𝛼𝑙 =

𝑙2 + 𝑙12

Compliant Robotics Peking University, Globex, July 201821

Constant curvature in 2D

𝑙2 − 𝑙1 = 2𝜃𝑑

𝑑𝑟

𝜃

𝑑

𝑙1

𝑙2

x

y

x1

y1

−𝜃

o1

o 𝛼

𝜃 =𝑙2 − 𝑙12𝑑

𝑟 =1

𝑘=

2𝑙1𝑑

𝑙2−𝑙1

𝑙1 = 𝜃𝑟𝑙2 = 𝜃 𝑟 + 2𝑑𝑙 = 𝜃(𝑟 + 𝑑)

Compliant Robotics Peking University, Globex, July 201822

Constant curvature in 3D

bending plane

base plane

Compliant Robotics Peking University, Globex, July 201823

Arc parameters: Length

• Given the bending geometry it can be seen that

Constant curvature in 3D

𝑑 ∙ cosΦi

𝑟

𝜃

𝑟𝑖

bending plane base plane

Compliant Robotics Peking University, Globex, July 201824

Arc parameters: Length

• Substituting the arc lengths and this relation becomes

Constant curvature in 3D

Compliant Robotics Peking University, Globex, July 201825

Arc parameters: Length

• The relations between and the bending planecan be denoted as

• Substituting these relations into yields the expression

Constant curvature in 3D

Compliant Robotics Peking University, Globex, July 201826

Arc parameters: Bending angle

• To obtain the bending angle, the previously derived equation

is to be rearranged for actuators 1&2 and equated leading to

• This procedure can be repeated for all actuator pairs and rearranged, leading to the expression

Constant curvature in 3D

Compliant Robotics Peking University, Globex, July 201827

Proof

sin ( α ± β ) = sinα cosβ ± cosα sinβ cos ( α ± β ) = cosα cosβ ∓ sinα sinβ

cos 𝜙1 = cos𝜋

2− 𝜙 = 𝑠𝑖𝑛𝜙

cos 𝜙3 = cos11𝜋

6− 𝜙 = 𝑐𝑜𝑠

11𝜋

6𝑐𝑜𝑠𝜙 + sin

11𝜋

6𝑠𝑖𝑛𝜙

3

2𝑐𝑜𝑠𝜙 −

1

2𝑠𝑖𝑛𝜙

cos 𝜙2 = cos7𝜋

6− 𝜙 = 𝑐𝑜𝑠

7𝜋

6𝑐𝑜𝑠𝜙 + sin

7𝜋

6𝑠𝑖𝑛𝜙

−3

2𝑐𝑜𝑠𝜙 −

1

2𝑠𝑖𝑛𝜙

Recall

Compliant Robotics Peking University, Globex, July 201828

Proof

𝜃𝑑 =𝑙2 − 𝑙1

𝐶𝑂𝑆𝜙1 − 𝐶𝑂𝑆𝜙2

𝜃𝑑 =𝑙3 − 𝑙1

𝐶𝑂𝑆𝜙1 − 𝐶𝑂𝑆𝜙3

3

2𝑠𝑖𝑛𝜙 +

3

2𝑐𝑜𝑠𝜙 = (𝑙2 − 𝑙1)/𝜃𝑑

3

2𝑠𝑖𝑛𝜙 −

3

2𝑐𝑜𝑠𝜙 = (𝑙3 − 𝑙1)/𝜃𝑑

(1)

(2)

(1)+(2)

(1)-(2)

3𝑠𝑖𝑛𝜙 = (𝑙2 + 𝑙3 − 2𝑙1)/𝜃𝑑

3𝑐𝑜𝑠𝜙 = (𝑙2 − 𝑙3)/𝜃𝑑

𝑡𝑎𝑛𝜙 =3

3

(𝑙2 + 𝑙3 − 2𝑙1)

(𝑙2 − 𝑙3)

Compliant Robotics Peking University, Globex, July 201829

Arc parameters: Curvature

Based on geometric observations it can be derived that

Substituting into and (r=1/K) yields

Consider actuator 1, 𝜙1 = 90 − 𝜙

𝜅 =𝑙2 + 𝑙3 − 2𝑙1

𝑙1 + 𝑙2 + 𝑙3 𝑑 c𝑜𝑠Φ1

Constant curvature in 3D

Compliant Robotics Peking University, Globex, July 201830

Consider actuator 1, 𝜙1 = 90 − 𝜙

𝜅 =𝑙2 + 𝑙3 − 2𝑙1

𝑙1 + 𝑙2 + 𝑙3 𝑑 sin𝛷

Constant curvature in 3D

Compliant Robotics Peking University, Globex, July 201831

Arc parameters: Curvature

Given the curvature derivation and previous expression for the bending with the identity

sin(tan−1𝑦

𝑥) = 𝑦/ 𝑥2 + 𝑦2

It can be seen that

Constant curvature in 3D

Compliant Robotics Peking University, Globex, July 201832

Overview

Arc parameters of a 3dof continuum robot with actuator lengths 𝑙𝑖 can be represented as

Forward mapping

Compliant Robotics Peking University, Globex, July 201833

Arc geometry

• Shape of the robot can be expressed applying the transformation

Where R represent rotation matrix and p is the translation vector

Mapping: Config to Task space-2D

𝐴10 =

𝑹 𝒑0 1

Compliant Robotics Peking University, Globex, July 201834

Mapping: Config to Task space-2D

𝑙 =𝑙2 + 𝑙12

𝜃 =𝑙2 − 𝑙12𝑑

𝑑𝑟

𝜃

𝑑

𝑙1

𝑙2

x

y

x1

y1

−𝜃

o1

o 𝛼

𝛼 =𝜋

2−𝜃

2𝜃

2

Compliant Robotics Peking University, Globex, July 201835

Mapping: Config to Task space-2D

𝑙 =𝑙2 + 𝑙12

𝜃 =𝑙2 − 𝑙12𝑑

𝑑𝑟

𝜃

𝑑

𝑙1

𝑙2

x

y

x1

y1

−𝜃

o1

o 𝛼

𝛼 =𝜋

2−𝜃

2

𝑜𝑜1 = 2𝑙

𝜃𝑐𝑜𝑠𝛼

𝑜1 =2𝑙

𝜃𝑐𝑜𝑠𝛼 2,

2𝑙

𝜃𝑐𝑜𝑠𝛼𝑠𝑖𝑛𝛼

𝑇

𝜃

2

Compliant Robotics Peking University, Globex, July 201836

Mapping: Config to Task space-2D

𝑑𝑟

𝜃

𝑑

𝑙1

𝑙2

x

y

x1

y1

−𝜃

o1

o 𝛼

𝛼 =𝜋

2−𝜃

2

𝑜1 =2𝑙

𝜃𝑐𝑜𝑠𝛼 2,

2𝑙

𝜃𝑐𝑜𝑠𝛼𝑠𝑖𝑛𝛼

𝑇

𝑅 =cos(−𝜃) −sin(− 𝜃)sin(−𝜃) cos(−𝜃)

𝐴10 = 𝑅 𝑜1

0 1

𝑝01

= 𝐴10 𝑝1

1

Compliant Robotics Peking University, Globex, July 201837

Mapping: Config to Task space-2D- Singularity

𝑑𝑟

𝜃

𝑑

𝑙1

𝑙2

x

y

x1

y1

−𝜃

o1

o 𝛼

𝛼 =𝜋

2−𝜃

2

𝑜1 =2𝑙

𝜃𝑐𝑜𝑠𝛼 2,

2𝑙

𝜃𝑐𝑜𝑠𝛼𝑠𝑖𝑛𝛼

𝑇

𝜃 =𝑙2 − 𝑙12𝑑

Singularity problem:

When θ is zero, o1 become undefined

Compliant Robotics Peking University, Globex, July 201838

Programming exercise in class

change name “transcc2D_q.m” to “transcc2D.m”

complete this function and type in the commend window:

[A o1]=transcc2D(pi/4)

results:A = 0.7071 0.7071 3.7292

-0.7071 0.7071 9.0032 0 0 1.0000

o1 = 3.7292 9.0032

Mapping: Config to Task space-2D

Compliant Robotics Peking University, Globex, July 201839

Programming exercise in class

change name “transcc2D_q.m” to “transcc2D_L.m”

change this function to : [A o1]=transcc2D_L(L1, L2)

Input variables are the two tendon lengths L1 and L2

Mapping: Config to Task space-2D

Compliant Robotics Peking University, Globex, July 201840

Mapping: Config to Task space-2D- Singularity

𝑑𝑟

𝜃

𝑑

𝑙1

𝑙2

x

y

x1

y1

−𝜃

o1

o 𝛼

𝛼 =𝜋

2−𝜃

2

𝑜1 =2𝑙

𝜃𝑐𝑜𝑠𝛼 2,

2𝑙

𝜃𝑐𝑜𝑠𝛼𝑠𝑖𝑛𝛼

𝑇

𝜃 =𝑙2 − 𝑙12𝑑

(𝑟 + 𝑑) =1

𝑘=

(𝑙1+𝑙2)𝑑

𝑙2−𝑙1

Recall: 𝑟 =1

𝑘=

2𝑙1𝑑

𝑙2−𝑙1

Compliant Robotics Peking University, Globex, July 201841

Mapping: Config to Task space-2D- better solution

𝑑𝑟

𝜃

𝑜1 = 𝑟 − 𝑟𝑐𝑜𝑠𝜃, 𝑟𝑠𝑖𝑛𝜃 𝑇

z

x

o1

𝑟 =1

𝑘=

(𝑙1+𝑙2)𝑑

𝑙2−𝑙1

Compliant Robotics Peking University, Globex, July 201842

In-plane translational vector p

Constant curvature in 3D

𝑑 ∙ cosΦ

𝑟

𝜃

𝑟𝑖

𝒑 = 𝑟 − 𝑟𝑐𝑜𝑠𝜃, 0, 𝑟𝑠𝑖𝑛𝜃 𝑇

z

x’

x’

⦿x

Compliant Robotics Peking University, Globex, July 201843

Arc geometry

Shape of the robot can be expressed applying the transformation

Where Ri represent rotation matrices about axis i and

Mapping: Config to Task space-3D

Compliant Robotics Peking University, Globex, July 201844

Mapping: Config to Task space-3D

𝑅𝑦 𝜃 =cos 𝜃 0 sin 𝜃0 1 0

−sin 𝜃 0 cos 𝜃𝑅𝑧 𝜙 =

cos𝜙 −sin𝜙 0sin𝜙 cos 𝜙 00 0 1

Compliant Robotics Peking University, Globex, July 201845

Mapping: Config to Task space-3D

Aac= Aab Abc =Recap

𝑅𝑧 𝜙 =cos𝜙 −sin𝜙 0sin𝜙 cos𝜙 00 0 1

𝑅𝑦 𝜃 =cos 𝜃 0 sin 𝜃0 1 0

−sin 𝜃 0 cos 𝜃

Rt =cos𝜙𝑐𝑜𝑠𝜃 −sin 𝜃 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃sin𝜙𝑐𝑜𝑠𝜃 cos𝜙 𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃−𝑠𝑖𝑛𝜃 0 𝑐𝑜𝑠𝜃

𝑝𝑡 =𝑐𝑜𝑠𝜙𝑟(1 − 𝑐𝑜𝑠𝜃)𝑠𝑖𝑛𝜙𝑟(1 − 𝑐𝑜𝑠𝜃)

𝑟𝑠𝑖𝑛𝜃

Compliant Robotics Peking University, Globex, July 201846

Mapping: Config to Task space-3D

Rt =cos𝜙𝑐𝑜𝑠𝜃 −sin 𝜃 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃sin𝜙𝑐𝑜𝑠𝜃 cos𝜙 𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃−𝑠𝑖𝑛𝜃 0 𝑐𝑜𝑠𝜃

𝑝𝑡 =𝑐𝑜𝑠𝜙𝑟(1 − 𝑐𝑜𝑠𝜃)𝑠𝑖𝑛𝜙𝑟(1 − 𝑐𝑜𝑠𝜃)

𝑟𝑠𝑖𝑛𝜃

T =

cos𝜙𝑐𝑜𝑠𝜃 −sin 𝜃 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃sin𝜙𝑐𝑜𝑠𝜃 cos𝜙 𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃−𝑠𝑖𝑛𝜃0

00

𝑐𝑜𝑠𝜃0

𝑐𝑜𝑠𝜙𝑟(1 − 𝑐𝑜𝑠𝜃)𝑠𝑖𝑛𝜙𝑟(1 − 𝑐𝑜𝑠𝜃)

𝑟𝑠𝑖𝑛𝜃1

Compliant Robotics Peking University, Globex, July 201847

Mapping: Config to Task space-3D

𝑘𝑙 = 𝜃

𝑑 ∙ cosΦ

𝑟

𝜃

𝑟𝑖

𝑟 =1

𝑘

S

θs

𝑘𝑠 = 𝜃𝑠

Let a point along the manipulator central

curve with arc length s

Arbitrary location on the continuum body

Compliant Robotics Peking University, Globex, July 201848

The manipulator-independent mapping function from configuration to task space can be summarized by

Mapping: Config to Task space

Compliant Robotics Peking University, Globex, July 201849

Recap: derivation from DH

• Same expression can be derived following DH convention

• Element along curve can be expressed by number of transformations

Mapping: Config to Task space

Compliant Robotics Peking University, Globex, July 201850

• Inverse kinematic formulation is not trivial, particularly for multi-segment manipulators with large number of segments

• For Task to Configuration space mapping solution exist based on

– A closed geometric formulation (Neppalli et al. 2008)

– Jacobian derivation ( “differential kinematics”)

Inverse mapping

Compliant Robotics Peking University, Globex, July 201851

• The Configuration to Actuator space mapping is similarly to FK highly individualized

• The solution exist based on

– A analytical geometric formulation

– Jacobian derivation ( “differential kinematics”)

Inverse mapping

Compliant Robotics Peking University, Globex, July 201852

Programming exercise

cc_1seg_execise_q.m

put cc_1seg_execise_q.m and transcc2D.m into the same folder

%tendon1

l1=12;

%tendon2

l2=8;

%pex is the coordinates of

unit vector of x axis in

the local frame of the tip

pex=[1 0]'

central bending axis of the arm

Compliant Robotics Peking University, Globex, July 201853

Try different combination of l1 and l2

What happens if l1=l2?

Testing the limit