#### 期刊菜单

Multi-Objective Optimization and Redundant Joint Planning of Minimally Invasive Surgical Robot Master Hand
DOI: 10.12677/MOS.2024.131069, PDF, HTML, XML, 下载: 96  浏览: 132  国家自然科学基金支持

Abstract: As the carrier of the interaction between the doctor and the minimally invasive surgical robot sys-tem, the performance of the master hand will directly affect the surgical quality. In order to im-prove the comprehensive performance of the master hand, a multi-objective genetic algorithm is proposed to solve the multi-objective optimization model, the model consists of three optimization variables (the length of three connecting rods of the master hand), three optimization objective functions (kinematic performance index, stiffness performance index, dynamic performance index) and one constraint condition (the length of the connecting rod constraint); in order to make the wrist have the function of realizing cooperative tasks during operation, a redundant joint is added to the three-degree-of-freedom wrist, the optimal angle of redundant joint is obtained by weighted minimum norm method, and then the angle of other joints is obtained by analytic method.

1. 引言

2. 主手运动学建模

2.1. 正运动学分析

Figure 1. Master hand model diagram

Figure 2. Master hand D-H coordinate system

Table 1. Master hand D-H parameter table

${}_{7}{}^{0}T={}_{1}{}^{0}T{}_{2}{}^{1}T{}_{3}{}^{2}T{}_{4}{}^{3}T{}_{5}{}^{4}T{}_{6}{}^{5}T{}_{7}{}^{6}T=\left[\begin{array}{cccc}{n}_{x}& {o}_{x}& {a}_{x}& {p}_{x}\\ {n}_{y}& {o}_{y}& {a}_{y}& {p}_{y}\\ {n}_{z}& {o}_{z}& {a}_{z}& {p}_{z}\\ 0& 0& 0& 1\end{array}\right]$ (1)

${p}_{x}={c}_{1}\left({a}_{3}{c}_{23}+{a}_{2}{c}_{2}+{d}_{4}{s}_{23}\right)+{d}_{3}{s}_{1}$ (2)

${p}_{y}={s}_{1}\left({a}_{3}{c}_{23}+{a}_{2}{c}_{2}+{d}_{4}{s}_{23}\right)+{d}_{3}{c}_{1}$ (3)

${p}_{z}={a}_{2}{s}_{2}+{a}_{3}{s}_{23}-{d}_{4}{c}_{23}$ (4)

2.2. 逆运动学分析

${}^{0}P{}_{7}={}^{0}P{}_{4}={}^{0}T{}_{1}{}^{1}T{}_{2}{}^{2}T{}_{3}{}^{3}P{}_{4}={}^{0}T{}_{1}{}^{1}T{}_{2}{}^{2}T{}_{3}\left[\begin{array}{c}{a}_{3}\\ -{d}_{4}\\ 0\\ 1\end{array}\right]={}^{0}T{}_{1}{}^{1}T{}_{2}\left[\begin{array}{c}{f}_{1}\\ {f}_{2}\\ {f}_{3}\\ 1\end{array}\right]={}^{0}T{}_{1}\left[\begin{array}{c}{g}_{1}\\ {g}_{2}\\ {g}_{3}\\ 1\end{array}\right]$ (5)

$\begin{array}{l}{f}_{1}={a}_{3}{c}_{3}+{d}_{4}{s}_{3}+{a}_{2}\\ {f}_{2}={a}_{3}{s}_{3}-{d}_{4}{c}_{3}\\ {f}_{3}={d}_{3}\end{array}$ (6)

$\begin{array}{l}{g}_{1}={f}_{1}{c}_{2}-{f}_{2}{s}_{2}\\ {g}_{2}=-{f}_{3}\\ {g}_{3}={f}_{1}{s}_{2}+{f}_{2}{c}_{2}\end{array}$ (7)

(1) 求解θ3

${d}_{4}{s}_{3}+{a}_{3}{c}_{3}=k$ (8)

$k=\frac{{p}_{x}^{2}+{p}_{y}^{2}+{p}_{z}^{2}-{d}_{3}^{2}-{d}_{4}^{2}-{a}_{2}^{2}-{a}_{3}^{2}}{2{a}_{2}}$ (9)

${a}_{3}=\rho \mathrm{cos}\varphi ,{d}_{4}=\rho \mathrm{sin}\varphi$ (10)

$\mathrm{sin}\left({\theta }_{3}+\varphi \right)=\frac{k}{\rho }$ (11)

$\mathrm{cos}\left({\theta }_{3}+\varphi \right)=±\sqrt{1-{\left(\frac{k}{\rho }\right)}^{2}}$ (12)

${\theta }_{3}=a\mathrm{tan}2\left(k,±\sqrt{{\rho }^{2}-{k}^{2}}\right)-\varphi$ (13)

$\rho =\sqrt{{a}_{3}^{2}+{d}_{4}^{2}},\varphi =a\mathrm{tan}2\left({a}_{3},{d}_{4}\right)$ (14)

(2) 求解θ2

${p}_{z}={f}_{1}\mathrm{sin}{\theta }_{2}+{f}_{2}\mathrm{cos}{\theta }_{2}$ (15)

${\theta }_{2}=a\mathrm{tan}2\left({p}_{z},±\sqrt{{f}_{1}^{2}+{f}_{2}^{2}-{p}_{z}^{2}}\right)-a\mathrm{tan}2\left({f}_{1},{f}_{2}\right)$ (16)

(3) 求解θ1

${p}_{y}={g}_{1}\mathrm{sin}{\theta }_{1}+{g}_{2}\mathrm{cos}{\theta }_{1}$ (17)

${\theta }_{1}=a\mathrm{tan}2\left({p}_{y},±\sqrt{{g}_{1}^{2}+{g}_{2}^{2}-{p}_{y}^{2}}\right)-a\mathrm{tan}2\left({g}_{1},{g}_{2}\right)$ (18)

(4) 求解θ4

(5) 求解θ5，θ6和θ7

$\begin{array}{c}{}^{4}T{}_{7}={}^{4}T{}_{5}{}^{5}T{}_{6}{}^{6}T{}_{7}=\left[\begin{array}{cccc}\sim & \sim & {c}_{5}{s}_{6}& \sim \\ -{s}_{6}{c}_{7}& {s}_{6}{s}_{7}& {c}_{6}& \sim \\ \sim & \sim & {s}_{5}{s}_{6}& \sim \\ 0& 0& 0& 1\end{array}\right]\\ ={}^{3}T{{}_{4}}^{-1}{}^{2}T{{}_{3}}^{-1}{}^{1}T{{}_{2}}^{-1}{}^{0}T{{}_{1}}^{-1}{}^{0}T{}_{7}\end{array}$ (19)

${\theta }_{5}=a\mathrm{tan}2\left({}^{4}T{}_{7}\left(3,3\right),{}^{4}T{}_{7}\left(1,3\right)\right)$ (20)

${\theta }_{6}=a\mathrm{tan}2\left({}^{4}T{}_{7}\left(1,3\right)/{c}_{5},{}^{4}T{}_{7}\left(2,3\right)\right)$ (21)

${\theta }_{7}=a\mathrm{tan}2\left({}^{4}T{}_{7}\left(2,2\right),-{}^{4}T{}_{7}\left(2,1\right)\right)$ (22)

3. 主手多目标优化

3.1. 各性能指标分析

3.1.1. 运动学性能指标

$w=\sqrt{\mathrm{det}\left(J\left(\theta \right){J}^{\text{T}}\left(\theta \right)\right)}$ (23)

${f}_{1}=\frac{\underset{i=1}{\overset{n}{\sum }}1/{w}_{i}}{n\mathrm{max}\left(1/w\right)}$ (24)

3.1.2. 刚度性能指标

${}_{i}{}^{B}C=\left[\begin{array}{cc}{}_{i}{}^{B}R& -{}_{i}{}^{B}R\left[{}^{i}P{}_{T}×\right]\\ 0& {}_{i}{}^{B}R\end{array}\right]{}_{i}{}^{i}C{\left[\begin{array}{cc}{}_{i}{}^{B}R& -{}_{i}{}^{B}R\left[{}^{i}P{}_{T}×\right]\\ 0& {}_{i}{}^{B}R\end{array}\right]}^{\text{T}}$ (25)

${}^{B}C=\underset{i=1}{\overset{n}{\sum }}{}^{B}C{}_{i}$ (26)

$K={}^{B}C{}^{-1}=\left[\begin{array}{cc}{K}_{t}& {K}_{tr}\\ {K}_{rt}& {K}_{r}\end{array}\right]$ (27)

${k}_{r}=‖{K}_{r}‖‖{K}_{r}{}^{-1}‖$ (28)

${S}_{r}=\frac{1-\left(1/{k}_{r}\right)}{\mathrm{max}\left(1/{k}_{r}\right)}$ (29)

${f}_{2}=\frac{\underset{i=1}{\overset{n}{\sum }}{S}_{ri}}{n\mathrm{max}\left({S}_{r}\right)}$ (30)

3.1.3. 动力学性能指标

$\tau =M\left(q\right)\stackrel{¨}{q}+C\left(q,\stackrel{˙}{q}\right)\stackrel{˙}{q}+g\left(q\right)$ (31)

$\tau =M\left(q\right)\stackrel{¨}{q}+g\left(q\right)$ (32)

${f}_{3}=\frac{\underset{i=1}{\overset{n}{\sum }}{\left(\underset{j=1}{\overset{m}{\sum }}\tau \left(j\right)\right)}_{i}}{n\mathrm{max}\left(\underset{j=1}{\overset{m}{\sum }}\tau \left(j\right)\right)}$ (33)

3.2. 优化模型建立

3.2.1. 优化目标

$\begin{array}{c}{f}_{1}=\underset{i=1}{\overset{n}{\sum }}1/{w}_{i}/n\mathrm{max}\left(1/w\right)\\ {f}_{2}=\underset{i=1}{\overset{n}{\sum }}{S}_{ri}/n\mathrm{max}\left({S}_{r}\right)\\ {f}_{3}=\underset{i=1}{\overset{n}{\sum }}{\left(\underset{j=1}{\overset{m}{\sum }}\tau \left(j\right)\right)}_{i}/n\mathrm{max}\left(\underset{j=1}{\overset{m}{\sum }}\tau \left(j\right)\right)\end{array}$ (34)

3.2.2. 约束条件

$680\text{mm}\le {a}_{2}+{a}_{3}\le 750\text{mm}$ (35)

Table 2. System resulting data of standard experiment

3.3. 优化算法选择

Figure 3. Correlation matrix graph

1. f1与f2，f3之间的相关系数为−0.10，0.05，三个目标函数之间相关性并不大，因此三个目标函数可以独立地表示力反馈主手的不同性能。

2. a2与f1，f2，f3之间的相关系数为0.24，−0.42，−0.15，说明a2与f2，f3负相关，与f1正相关。

3. a3与f1，f2，f3之间的相关系数为0.11，0.44，0.29，说明a2与f1，f2，f3正相关。

4. d4与f1，f2，f3之间的相关系数为−0.16，0.24，0.52，说明a2与f2，f3正相关，与f1负相关。

$F=\mathrm{min}\left({f}_{1}+{f}_{2}+{f}_{3}\right)$ (36)

${a}_{2}=370\text{mm},{a}_{3}=360\text{mm},{d}_{4}=-150\text{mm}$ (37)

Table 3. Partial optimization result

4. 冗余关节规划

4.1. 加权最小范数法

$\stackrel{˙}{x}=J\stackrel{˙}{q}$ (38)

$\stackrel{˙}{x}={\left({v}_{x},{v}_{y},{v}_{z},{\omega }_{\alpha },{\omega }_{\beta },{\omega }_{\gamma }\right)}^{\text{T}}$ (39)

$x={\left[{p}_{x},{p}_{y},{p}_{z},\alpha ,\beta ,\gamma \right]}^{\text{T}}$ (40)

$\stackrel{˙}{x}={\left[\frac{\text{d}{p}_{x}}{\text{d}t},\frac{\text{d}{p}_{y}}{\text{d}t},\frac{\text{d}{p}_{z}}{\text{d}t},\frac{\text{d}\alpha }{\text{d}t},\frac{\text{d}\beta }{\text{d}t},\frac{\text{d}\gamma }{\text{d}t}\right]}^{\text{T}}$ (41)

$‖\stackrel{˙}{\theta }‖=\sqrt{{\stackrel{˙}{\theta }}^{\text{T}}W\stackrel{˙}{\theta }}$ (42)

$W=\left[\begin{array}{cccc}{w}_{1}& 0& ...& 0\\ 0& {w}_{2}& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& {w}_{7}\end{array}\right]$ (43)

${J}_{W}=J{W}^{-\frac{1}{2}},{\stackrel{˙}{\theta }}_{W}={W}^{\frac{1}{2}}\stackrel{˙}{\theta }$ (44)

${w}_{i}=\left\{\begin{array}{l}1+|\frac{\partial U\left(\theta \right)}{\partial {\theta }_{i}}|,\text{}\Delta |\frac{\partial U\left(\theta \right)}{\partial {\theta }_{i}}|\text{​}\text{​}\text{​}\text{​}\text{}\ge 0\text{​}\text{​}\hfill \\ 1,\text{}\text{}\Delta ‖\frac{\partial U\left(\theta \right)}{\partial {\theta }_{i}}‖\text{​}\text{​}\le 0\hfill \end{array}$ (45)

$\Delta |\frac{\partial U\left(\theta \right)}{\partial {\theta }_{i}}|=|\frac{\partial U\left(\theta \left(t\right)\right)}{\partial {\theta }_{i}\left(t\right)}|-|\frac{\partial U\left(\theta \left(t-\Delta t\right)\right)}{\partial {\theta }_{i}\left(t-\Delta t\right)}|$ (46)

$U\left(\theta \right)=\underset{i=1}{\overset{7}{\sum }}\frac{1}{4}\frac{{\left({\theta }_{i\mathrm{max}}-{\theta }_{i\mathrm{min}}\right)}^{2}}{\left({\theta }_{i\mathrm{max}}-{\theta }_{i}\right)\left({\theta }_{i}-{\theta }_{i\mathrm{min}}\right)}$ (47)

$\nabla U\left(\theta \right)=\underset{i=1}{\overset{7}{\sum }}\frac{1}{4}\frac{{\left({\theta }_{i\mathrm{max}}-{\theta }_{i\mathrm{min}}\right)}^{2}\left(2{\theta }_{i}-{\theta }_{i\mathrm{max}}-{\theta }_{i\mathrm{min}}\right)}{{\left({\theta }_{i\mathrm{max}}-{\theta }_{i}\right)}^{2}{\left({\theta }_{i}-{\theta }_{i\mathrm{min}}\right)}^{2}}$ (48)

$\stackrel{˙}{x}={J}_{W}{\stackrel{˙}{\theta }}_{W}$ (49)

${\stackrel{˙}{\theta }}_{W}={J}_{W}^{\text{T}}{\left({J}_{W}{J}_{W}^{\text{T}}\right)}^{-1}\stackrel{˙}{x}$ (50)

$\stackrel{˙}{\theta }={W}^{-1}{J}^{\text{T}}{\left(J{W}^{-1}{J}^{\text{T}}\right)}^{-1}\stackrel{˙}{x}$ (51)

$\theta \left(t\right)=\theta \left(t-\Delta t\right)+\stackrel{˙}{\theta }\Delta t$ (52)

Figure 4. Redundant joint planning flowchart

4.2. 仿真验证

$x={\left[360,300,-220,{90}^{\circ },{0}^{\circ },{0}^{\circ }\right]}^{\text{T}}$ (53)

$\theta ={\left[{0}^{\circ },-{90}^{\circ },{90}^{\circ },{90}^{\circ },{0}^{\circ },{90}^{\circ },{0}^{\circ }\right]}^{\text{T}}$ (54)

${x}_{f}={\left[460,300,-170,{115}^{\circ },-{19}^{\circ },{28}^{\circ }\right]}^{\text{T}}$ (55)

$v={\left[10,0,5,3.5,-1.9,2.8\right]}^{\text{T}}$ (56)

Table 4. System resulting data of standard experiment

Figure 5. Joint angle change

$Y=A\mathrm{tan}2\left(\sqrt{{n}_{z}^{2}+{o}_{z}^{2}},{a}_{z}\right)$ (57)

$Z=A\mathrm{tan}2\left({a}_{y}/sY,{a}_{x}/sY\right)$ (58)

$X=A\mathrm{tan}2\left({o}_{z}/sY,-{n}_{z}/sY\right)$ (59)

$err=\frac{{p}_{u}-{p}_{s}}{{p}_{u}}×100%$ (60)

Figure 6. Relative error

Figure 7. End position error

${\epsilon }_{err}=\sqrt{{\left({p}_{dx}-{p}_{x}\right)}^{2}+{\left({p}_{dy}-{p}_{y}\right)}^{2}+{\left({p}_{dz}-{p}_{z}\right)}^{2}}$ (61)

5. 结束语

 [1] Rosen, J., Hannaford, B., Richards, C.G. and Sinanan, M.N. (2001) Markov Modeling of Minimally Invasive Surgery Based on Tool/Tissue Interaction and Force/Torque Signatures for Evaluating Surgical Skills. IEEE Transactions on Biomedical Engi-neering, 48, 579-591. https://doi.org/10.1109/10.918597 [2] Yoshikawa T. (1985) Manipulability of Robotic Mecha-nisms. The International Journal of Robotics Research, 4, 3-9. https://doi.org/10.1177/027836498500400201 [3] Salisbury, J.K. and Craig, J.J. (1982) Articulated Hands: Force Con-trol and Kinematic Issues. The International Journal of Robotics Research, 1, 4-17. https://doi.org/10.1177/027836498200100102 [4] 石志新, 等. 机器人机构的全域性能指标研究[J]. 机器人, 2005, 27(5): 420-422. [5] Zhang, Y., et al. (2014) Analysis and Verification of Repetitive Motion Planning and Feedback Control for Omnidirectional Mobile Manipulator Robotic Systems. Journal of Intelligent & Robotic Systems, 75, 393-411. https://doi.org/10.1007/s10846-014-0022-0 [6] Lynch, K.M. (2017) Modern Robotics. Cambridge University Press, Cambridge. [7] Siciliano, B. (2016) Springer Handbook of Robotics. Springer, Berlin. https://doi.org/10.1007/978-3-319-32552-1 [8] Guo, D. and Zhang, Y. (2014) Acceleration-Level Inequality-Based MAN Scheme for Obstacle Avoidance of Redundant Robot Manipulators. IEEE Transactions on Industrial Electronics, 61, 6903-6914. https://doi.org/10.1109/TIE.2014.2331036 [9] Kumar, A. and Kumar, V. (2017) Evolving an Interval Type-2 Fuzzy PID Controller for the Redundant Robotic Manipulator. Expert Systems with Applications, 73, 161-177. https://doi.org/10.1016/j.eswa.2016.12.029 [10] Maja, Z. and Vincent, H. (2013) Generalized Humanoid Leg Inverse Kinematics to Deal with Singularities, 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, 6-10 May 2013, 4791-4796. https://doi.org/10.1109/ICRA.2013.6631260 [11] Chen, D. and Zhang, Y. (2017) A Hybrid Multi-Objective Scheme Applied to Redundant Robot Manipulators. IEEE Transactions on Automation Science and Engineering, 14, 1337-1350. https://doi.org/10.1109/TASE.2015.2474157 [12] 田溢汕, 李佳轩, 宋峰峰, 等. 超冗余机器人避障轨迹规划[J]. 汕头大学学报(自然科学版), 2023, 38(1): 23-34. [13] 张威, 柳洪义, 郭大忠. 冗余度机器人最小关节力矩的优化[J]. 组合机床与自动化加工技术, 2008(6): 17-18, 22. [14] 闫文彬, 孙雷. 一种七自由度冗余机械臂的逆运动学优化算法[J]. 系统科学与数学, 2015, 35(7): 823-837. [15] Peiper, D.L. (1968) The Kinematics of Manipulators under Computer Control. Ph.D. Thesis, Stanford University, Stanford.