visual servo control of cable- driven soft robotic manipulator

Autonomous Robot Lab robotics.sjtu.edu.cnDepartment of Automation

Shanghai Jiao Tong University

Visual Servo Control of Cable-driven Soft Robotic Manipulator

Hesheng Wang (王贺升)Department of Automation

Shanghai Jiao Tong UniversityChina


Outline

• Introduction• Background• System Design• Kinematics• Controller Design• Experimental results• Conclusion


Background

Minimally Invasive Surgery (MIS)

On the surface of the beating heart

Inside the beating heart


Background

Concentric Tube Robot

HARP

Heart Lander


Background

Not Soft Enough

Not Safe Enough

Not Practical Enough


Background

“Soft” robotic systems

-Conform to the surroundings

-Ease of operation

-”Fit” to Dynamic environments


Research Background

Soft robots: material, manufacturing, Control Theory, computer simulationApplications: minimally invasive surgery, military, exploration, rescue…

（1） Starfish robot （2）Quadruped deformable robot

（3） Caterpillar robot （4）Mechanical octopus tentacle


Characters of soft robot

Characters Rigid robotsRedundant

robotHard

continuum robotSoft robot

DOF Small Many Infinite Infinite

Material strain None None Low High

Accuracy Very High High High Low

Safety Low Low Medium High

Flexibility Low High High High

Compatibility with obstacles

None Good Medium Very good

Controllability Easy Medium Hard Hard

Positioning Easy Medium Hard Hard


System Design

Soft Robotic Manipulator

Kinematics - hyper-redundancy


System Design

System Composition

A soft manipulator

A drive base frame

Control and display system


System Design

The Soft Manipulator 30mm

silicone rubber (ECOFLEX™)

8 non-abrasive fiber cables (DyneemaTM)

plastic caps

ablation tools and a micro CCD camera


System Design

Linear motion system

Rotational motion system

The Drive Base Frame


System Design

Joystick Control

Control and Display System

Cameral Display

Control Software


System Manipulation

Manipulate method

Manual Mode

The cable tension can be changed by setting the Head angle

Adding functional buttons

- functional buttons for each behavior

- buttons for selecting which cables are being controlled

- buttons for emergency

Automatic Mode

Path planning & Localization on the surface of the heart


System Manipulation Behavior Implementation

1. Blending

2. Contracting

3. Advancing/Retreating

4. Wriggling

5. Blending and Wriggling partly

6. S shape


Movie 5


Experiments

Unconstrained Environment-with a plastic thorax and a silicone heart

-different simulated paths on the surface

-to identify the geometrical shape and flexibility


Experiment Video


Experiments

Limited Environment- made of balloons to imitate the human tissue

- complicated path to guide the soft probe

- effects of the gravity and surroundings


Experiment Video 2


Experiment Video 3


Experiment Video 4


System Manipulation

KEY ISSUES

Kinematics Modeling- Influence of gravity

- Influence of Surroundings Environment

Dynamics Modeling- Modeling Method

- Calculation efficiency

Controller


Constant curvature hypothesis

r

denotes the angle between the bending plane and the positive direction of x axis,

denote the curvature angle of the bending plane respectively.

denote the curvature radius of the bending plane respectively.

three spaces and two mappings

Constant curvature hypothesis :dividing the whole body of the soft robotic manipulator into n segments, and each segment can be treated as a cylinder that the radius of section is constant


Kinematics

• For i-th segment– the length variables of 4 cables：

– the current length of 4 cables：

– the central axis of the i-th segment

1 2 3 4, , ,q q q q

31 2 4, , ,ll l ln n n n

ln


柔性机械臂运动学模型驱动空间-虚拟关节空间映射

2 23 1 4 2

2ii

q q q qnR

2 2

3 1 4 2

2 ii

L q Rr

q q q q

1 4 2

3 1

taniq qq q

Actuation space - Virtual joint space


柔性机械臂运动学模型虚拟关节空间-任务空间映射

11 2 3 4 5

2

2

1 1 1 11 1 1 1

0 0 0 1

ii i i i i i

i i i i i i i i i i

i i i i i i i i i i

i i i i i i i

T A A A A A

c c s c c c s rc cs c c s c s s r s c

c s s s c r s

0 1 11 2

iiT T T T

Virtual joint space – Task space


Perspective Projection

Cx

Cy

Cz u

v

),( 00 vu

C

u

v

C

O y

x

z

P

p

pCamera coordinate

frame

World coordinate frame

Normalized image plane

Physical retina

1)(

)(1

1)()(

ttz

tvtu

c

c

xΠ

0100

0sin

0

0cot

0

0

v

u

Π

angle between u and v axes

principal point

the magnifications


Projection Model

– the projection of the feature point on the image plane

– Interaction matrix

( )1( ( ))( ( )) 1

e x ty q t P

z q t

3( )

( ( ))1

eT x t

z q t m

( )1( ( )) ( ( ), ( ))( )( ( ))

v ty q t A y t q t

w tz q t

( )( ( )) ( ( ))

( )v t

z q t b q tw t


Property 1

For any homogenous vector , the product can be written in the following form:

where is a regressor matrix without depending on the unknown parameters.

ρ ρA )(t

))(,( tyρQ

σ)θyQ(ρ)ρA( )(, tt


Kinematic-based and Dyanmic Visual Servoing

Control law

Camera

+

-

Robot controller

Joint velocity

Visual information

Joint angle sensors

Robot Camera+

-

Robot

Visual information

Control law


Kinematic-based Visual Servoing

– Kinematic-based controller

– Adaptive law

1

1

ˆ( ) ( ( )) ( ( ), ( )) ( )1 ˆ( ( )) ( ( )) ( ) ( )2

T T

T T T

q t J q t A y t q t K y t

J q t b q t y t K y t

1ˆ( ) ( ( ), ( )) ( )b Tx t Y y t q t q t


Stability analysis

– Applying the image-based visual servo controller and adaptive algorithm, it can be proved that the position error of the feature point on the image plane will be convergent to zero when time approaches to the infinity

– a Lyapunov-like function is defined as follows:

– Finally

– By Babarrat’s Lemma, the stability could be proved.

lim ( ) 0t

y t

11 1( ) ( ( )) ( ) ( ) ( )2 2

T b T bV t y t K z q t y t x t x t

21

ˆ ˆ( ) ( ) ( ( ), ( )) ( ( ), ( )) ( )T T TV t y t D y t q t JK J D y t q t y t


Experimental system


Experimental results


实验与分析三种情况下的实验– Initial position：– Desired position：– Initial estimated feature 3D position：– gains：

0 208,166 Ty

50,150 Tdy

0 0.0 0.0 0.62 Tb x 6

1 3.0 10K 100

40 60 80 100 120 140 160 180 200 220

50

100

150

200

u(pixel)

v(pi

xel)

TrajectoryDesired position

The trajectory of the feature point on the image plane.

0 10 20 30 40 50 60-100

-50

0

50

100

150

200

Time(s)

Pos

ition

Erro

r(pix

el)

u

v

The image errors between current position and desired position.

Experimental Results


Conclusion and future work

• A cable-driven soft manipulator for cardiac ablation

-completely made of soft materials

-high degree of freedom

-high flexibility

• A modified behavior-based control method is presented crowded pericardial environment

• A kinematic model of the soft robotic manipulator with the concept of piecewise constant curvature is presented.

• An adaptive controller for image-based visual servoing of the soft robotic manipulator is developed.

• The performances of the proposed method are verified by experiments on a soft robotic manipulator.

• Future work includes considering dynamic visual servoing and environment effects on the robot.


Shanghai Jiao Tong University

Thank you!

visual servo control of cable- driven soft robotic manipulator

Documents