dynamic modeling of the da vinci research kit arm for the...
TRANSCRIPT
![Page 1: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/1.jpg)
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
![Page 2: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/2.jpg)
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
![Page 3: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/3.jpg)
3
Minimally Invasive Surgery
20/04/19 FrancescoPiqué,ThesisDiscussion
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
![Page 4: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/4.jpg)
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
20/04/19 FrancescoPiqué,ThesisDiscussion
![Page 5: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/5.jpg)
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
20/04/19 FrancescoPiqué,ThesisDiscussion
![Page 6: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/6.jpg)
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
![Page 7: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/7.jpg)
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
20/04/19 FrancescoPiqué,ThesisDiscussion
Joint positions 𝒒Joint velocities �̇�Joint accelerations �̈�
![Page 8: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/8.jpg)
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
20/04/19 FrancescoPiqué,ThesisDiscussion
Joint positions 𝒒Joint velocities �̇�Joint accelerations �̈�
![Page 9: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/9.jpg)
9
Dynamic Model
𝝉2 = 𝑴 𝒒 �̈� + 𝑪 𝒒, �̇� + 𝑮 𝒒 + 𝑲𝒆𝒒 + 𝑭𝒗�̇� + 𝑭𝒔𝑠𝑔𝑛 �̇� + 𝝉𝟎 + 𝑰𝒂�̈�
20/04/19 FrancescoPiqué,ThesisDiscussion
𝑲𝒆 = 𝑑𝑖𝑎𝑔{𝐾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
![Page 10: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/10.jpg)
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:
20/04/19 FrancescoPiqué,ThesisDiscussion
6x68 68x1
![Page 11: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/11.jpg)
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.
20/04/19 FrancescoPiqué,ThesisDiscussion
![Page 12: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/12.jpg)
Identification
𝛿∗ = 𝐴st ∗ bv
20/04/19 FrancescoPiqué,ThesisDiscussion 12
While the robot is performing the optimal trajectory, 𝒒, �̇�, �̈�, 𝝉 are collected, at afrequency of 100 Hz
![Page 13: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/13.jpg)
Identification
𝛿∗ = 𝐴st ∗ bv
While the robot is performing theoptimal trajectory, 𝒒, �̇�, �̈�, 𝝉 arecollected.
𝒀 𝒒𝟏, �̇�𝟏, �̈�𝟏⋮
𝒀 𝒒𝑴, �̇�𝑴, �̈�𝑴𝜹 =
𝝉𝟏⋮𝝉𝑴
The optimal parameter 𝜹∗ vector isobtained by pseudoinversion of Y,solving a least squares minimizationproblem.
20/04/19 FrancescoPiqué,ThesisDiscussion 13
M=n° ofsamples
![Page 14: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/14.jpg)
Identification
𝑌 𝒒 𝑡W , �̇�(𝑡W), �̈� 𝑡W ⋮
𝑌 𝒒 𝑡} , �̇�(𝑡}), �̈� 𝑡}𝜹 =
𝝉 𝑡W⋮
𝝉(𝑡})
20/04/19 FrancescoPiqué,ThesisDiscussion 14
𝑊 = 𝑑𝑖𝑎𝑔{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)
![Page 15: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/15.jpg)
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𝑖𝑛𝑡𝑒𝑟𝑎𝑐𝑡𝑖𝑜𝑛𝑤𝑟𝑒𝑛𝑐ℎ
20/04/19 FrancescoPiqué,ThesisDiscussion 15
𝑁 numberofsamples𝑖 jointnumber𝜏�i� maximumtorqueovertime𝜏��� minimumtorqueovertime
![Page 16: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/16.jpg)
16
Wrench Estimation Method
𝒘 = 𝐽5 �W ∗ 𝝉𝒆𝒙𝒕
DynamicModel
𝑞, �̇�, �̈�++
�̂� 𝜏565
𝜏345𝑤
𝐽5𝑖𝑛𝑡𝑒𝑟𝑎𝑐𝑡𝑖𝑜𝑛𝑤𝑟𝑒𝑛𝑐ℎ
In case of an interactionwrench:
𝝉V�� = 𝝉565 − 𝝉2
The external wrench can becalculated as:
20/04/19 FrancescoPiqué,ThesisDiscussion
![Page 17: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/17.jpg)
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
![Page 18: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/18.jpg)
18
Wrench Estimation Results
MeasuredWrenchEstimatedWrench
Fx Fy Fz Tx Ty Tz
NRMSD(%) 8.26 5.96 6.10 8.13 5.71 15.50
20/04/19 FrancescoPiqué,ThesisDiscussion
![Page 19: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/19.jpg)
19
Conclusions
AmodelofthePSMarmwasimplementedanditsparameterswereidentified.
Themodelwasvalidatedbyevaluatingitstorqueestimationabilityatthejointlevelandtheinteractionwrenchestimationability.
Futureworkmayincludethereflectionoftheestimatedforcestothemasterside.
![Page 20: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/20.jpg)
20
Thanks for your attention!
20/04/19 FrancescoPiqué,ThesisDiscussion
![Page 21: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/21.jpg)
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, �̇�𝟏, �̈�𝟏⋮
𝒀 𝒒𝑴, �̇�𝑴, �̈�𝑴
20/04/19 FrancescoPiqué,ThesisDiscussion
![Page 22: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/22.jpg)
2220/04/19 FrancescoPiqué,ThesisDiscussion
DynamicParameters
![Page 23: Dynamic Modeling of the Da Vinci Research Kit Arm for the ...nearlab.polimi.it/wp-content/uploads/2019/04/7_Pique_dynamic.pdfDynamic Modeling of the Da Vinci Research Kit Arm for the](https://reader036.vdocuments.us/reader036/viewer/2022081601/611582778cf69e656b24b0f2/html5/thumbnails/23.jpg)
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
𝛿∗ = 𝐴st ∗ bv
Recall themodel’s equation:𝝉 = 𝒀 𝑞, �̇�, �̈� 𝛿
OnlineOffline
20/04/19 FrancescoPiqué,ThesisDiscussion 23