Dynamic Modeling of the Da Vinci Research Kit Arm for the...

Dynamic Modeling of the Da VinciResearch Kit Arm for the Estimation

of the Interaction Wrench

1. Politecnico di Milano, Department of Electronics, Information and Bioengineering (DEIB)

2. The BioRobotics Institute, Scuola Superiore Sant’Anna

120/04/19

Advisor: Prof. Elena De Momi1Co-Advisor: Prof. Arianna Menciassi2

Supervisors: Mohamed Boushaki2,Margherita Brancadoro2

Candidate: Francesco PiquéID: 873785

2

Minimally Invasive Surgery (MIS)

20/04/19 FrancescoPiqué,ThesisDiscussion

MIS (laparoscopy) advantages withrespect to open surgery:

• Less trauma for patient ✓• Shorter recovery time ✓

• More difficulties for the surgeon✗Upright position: fatigueNo hand-eye coordination2D perceptionFulcrum effectNo hand palpation

3

Minimally Invasive Surgery


Improvements of robot-assisted MIS:

• Filtered hand tremor✓• Scaling of movements✓• Surgeon comfort and immersion✓

• Separation between patient andsurgeon : no feeling of the appliedforce✗

A haptic feedback is desirable for:• Lump detection• Suture knots

4

On board sensors are costly, cumbersomeand need to be sterilizable

A sensorless strategy is to reconstruct theinteraction wrench from the joint torques,which can be estimated from the currentsin the motors (Sang, 2017).

The dynamics of the slave arm must beconsidered

Introduction

a) Culjat, et al., EMBS, IEEE, 2008b) Fontanelli, et al. , IROS, IEEE 2017c) Talasaz, et al., BioRob, IEEE, 2012d) Seibold, et al. ICRA. IEEE, 2005


5

Goal

To estimate the interaction wrench from the joint torques, taking intoaccount the dynamics

• The dynamic model of the dVRK slave arm must be modeled• The parameters of the model must be defined and identified

Jointpositions𝒒Joint velocities�̇�

Jointaccelerations�̈�

DynamicModel

Estimatedjointtorques𝝉2


6

dVRK Hardware

Open version of the da Vinci System

Intended for research only

• 2 Master Tool Manipulators (MTM)

• 2 Patient Side Manipulators(PSM)

• High resolution stereo viewer (HRSV)

• Footpedal20/04/19 FrancescoPiqué,ThesisDiscussion

7

Dynamic Model

The total torque sensed at thejoint level consists of:

• dynamic torques 𝝉2, causingthe robot motion

• 𝝉345: Torques resisting to anexternal interaction wrench

DynamicModel ++

𝝉2 𝝉565

𝝉345𝐰 = [F;, F=, F>, T;, T=, T>]

𝐽5InteractionWrench


Joint positions 𝒒Joint velocities �̇�Joint accelerations �̈�

8

Dynamic Model

The total torque sensed at thejoint level consists of:

• dynamic torques 𝝉2, causingthe robot motion

• 𝝉345: Torques resisting to anexternal interaction wrench

DynamicModel ++

𝝉2 𝝉565

𝝉345𝐰 = [F;, F=, F>, T;, T=, T>]

𝐽5InteractionWrench


Joint positions 𝒒Joint velocities �̇�Joint accelerations �̈�

9

Dynamic Model

𝝉2 = 𝑴 𝒒 �̈� + 𝑪 𝒒, �̇� + 𝑮 𝒒 + 𝑲𝒆𝒒 + 𝑭𝒗�̇� + 𝑭𝒔𝑠𝑔𝑛 �̇� + 𝝉𝟎 + 𝑰𝒂�̈�


𝑲𝒆 = 𝑑𝑖𝑎𝑔{𝐾VW, 𝐾VX, 0, 𝐾VZ, 0, 0}

𝑭𝒗 = 𝑑𝑖𝑎𝑔 𝐹]W, … , 𝐹]Z, 𝑭𝒗𝒍 Viscousfriction

𝑭𝒔 = 𝑑𝑖𝑎𝑔 𝐹fW, … , 𝐹fg Static – Coulomb friction

𝑰𝒂 = 𝑑𝑖𝑎𝑔 𝐼iW, … , 𝐼igSimplified drive inertia

Inertia Matrix Coriolis &

centrifugalforces matrix

Joint elasticity, due to power cables and a torsional spring

Gravitation Matrix

𝜏kstatic friction offset

10

Dynamic Model

Expressions of Y and 𝜹 are computedusing the Newton Euler Algorithm

𝝉 = 𝒀 𝒒, �̇�, �̈� ∗ 𝜹

The model is linear w.r.t the vector 𝜹 of thedynamic parameters:


6x68 68x1

11

Kinematic Model

1 2 3

4

5

6

The Newton Euler Algorithmcomputes the dynamic model takingas input the robot DH parameters.

The DH parameters a, 𝛼,d, 𝜃, defineunivocally, for every degree offreedom, the kinematics of the robotarm.


Identification

𝛿∗ = 𝐴st ∗ bv

20/04/19 FrancescoPiqué,ThesisDiscussion 12

While the robot is performing the optimal trajectory, 𝒒, �̇�, �̈�, 𝝉 are collected, at afrequency of 100 Hz

Identification


While the robot is performing theoptimal trajectory, 𝒒, �̇�, �̈�, 𝝉 arecollected.

𝒀 𝒒𝟏, �̇�𝟏, �̈�𝟏⋮

𝒀 𝒒𝑴, �̇�𝑴, �̈�𝑴𝜹 =

𝝉𝟏⋮𝝉𝑴

The optimal parameter 𝜹∗ vector isobtained by pseudoinversion of Y,solving a least squares minimizationproblem.


M=n° ofsamples

Identification

𝑌 𝒒 𝑡W , �̇�(𝑡W), �̈� 𝑡W ⋮

𝑌 𝒒 𝑡} , �̇�(𝑡}), �̈� 𝑡}𝜹 =

𝝉 𝑡W⋮

𝝉(𝑡})


𝑊 = 𝑑𝑖𝑎𝑔{1

𝜏W,�i�, … ,

1𝜏g,�i�

}

𝜹∗ =𝑊 ∗ 𝑌 𝒒 𝑡W , �̇�(𝑡W), �̈� 𝑡X

⋮𝑊 ∗ 𝑌 𝒒 𝑡} , �̇�(𝑡}), �̈� 𝑡}

t

∗𝑊 ∗ 𝝉 𝑡W

⋮𝑊 ∗ 𝝉(𝑡})

Data Collection

Weighting Matrix Definition

Parameter Identification, bypseudo inversion of weighedmatrices

1)

2)

3)

Validation of the Dynamic Model

𝝉2 = 𝑌 𝒒, �̇�, �̈� ∗ 𝜹∗

Validation is done by estimating jointtorques in a free space motion.

Ideally, the estimated torques areequal to the sensed torques.

𝑁𝑅𝑀𝑆𝐷� =

1𝑁 ∑ �̂�� − 𝜏� �

X��

�

𝜏�i� − 𝜏��

DynamicModel

𝑞, �̇�, �̈�++

�̂� 𝜏565

𝜏345𝑤

𝐽5𝑖𝑛𝑡𝑒𝑟𝑎𝑐𝑡𝑖𝑜𝑛𝑤𝑟𝑒𝑛𝑐ℎ


𝑁 numberofsamples𝑖 jointnumber𝜏�i� maximumtorqueovertime𝜏�� minimumtorqueovertime

16

Wrench Estimation Method

𝒘 = 𝐽5 �W ∗ 𝝉𝒆𝒙𝒕

DynamicModel

𝑞, �̇�, �̈�++

�̂� 𝜏565

𝜏345𝑤

𝐽5𝑖𝑛𝑡𝑒𝑟𝑎𝑐𝑡𝑖𝑜𝑛𝑤𝑟𝑒𝑛𝑐ℎ

In case of an interactionwrench:

𝝉V�� = 𝝉565 − 𝝉2

The external wrench can becalculated as:


17

Validation

20/04/19

joints 1 2 3 4 5 6

NRMSD(%) 2.38 4.80 12.83 6.49 15.29 18.11

EstimatedTorquesMeasuredTorques

𝑁𝑅𝑀𝑆𝐷� =

1𝑁 ∑ �̂�� − 𝜏� �

X��

�

𝜏�i� − 𝜏��

FrancescoPiqué,ThesisDiscussion

18

Wrench Estimation Results

MeasuredWrenchEstimatedWrench

Fx Fy Fz Tx Ty Tz

NRMSD(%) 8.26 5.96 6.10 8.13 5.71 15.50


19

Conclusions

AmodelofthePSMarmwasimplementedanditsparameterswereidentified.

Themodelwasvalidatedbyevaluatingitstorqueestimationabilityatthejointlevelandtheinteractionwrenchestimationability.

Futureworkmayincludethereflectionoftheestimatedforcestothemasterside.

20

Thanks for your attention!


21

Excitation (Optimal Trajectory Generation)

The optimal excitation trajectory wascomputed offline, from the equation:

𝐿 = 5 Number of harmonics𝜔 = 2𝜋 ∗ 0.1 rad/s fundamental frequency𝑖 joint number

Parameters 𝑎°�, 𝑏°�, 𝑞�k are tuned to minimizethe conditioning number of:

𝑞� 𝑡 = ∑ i²³

´µ°sin 𝜔𝑙𝑡 − ·²

³

´µ°cos 𝜔𝑙𝑡 + 𝑞�k¸

°¹W

𝒀 𝒒W, �̇�𝟏, �̈�𝟏⋮

𝒀 𝒒𝑴, �̇�𝑴, �̈�𝑴



DynamicParameters

Identification

𝑞, �̇�, �̈�, 𝜏•• DataCollection•• SamplingRate100Hz•• Butterworth3rdorderLPFat10Hz

𝑌 𝑞», �̇�», �̈�»𝜏»

••One foreachtimesample

𝐴 =𝑌 𝑞W, �̇�W, �̈�W

⋮𝑌 𝑞}, �̇�}, �̈�}

, 𝑏 =𝜏W⋮𝜏}

••Build matrices

𝑊»

= 𝑑𝑖𝑎𝑔{1

max 𝜏W, … ,

1max 𝜏g

}

•• Weightingmatrix

𝐴s =𝑊W ∗ 𝑌 𝑞W, �̇�W, �̈�W

⋮𝑊} ∗ 𝑌 𝑞}, �̇�}, �̈�}

, 𝑏s =𝑊W ∗ 𝜏W

⋮𝑊} ∗ 𝜏}

••Weight theequations ••Linearleast

squares solution


Recall themodel’s equation:𝝉 = 𝒀 𝑞, �̇�, �̈� 𝛿

OnlineOffline


Dynamic Modeling of the Da Vinci Research Kit Arm for the...

Documents

Transcript of Dynamic Modeling of the Da Vinci Research Kit Arm for the...