微波消融外科手术机器人的正逆运动学分析

本文主要讲述本人所在项目组研制的微波消融外科手术机器人的正逆运动学分析，以更好的理解课上所讲述的坐标变换、运动分析等相关内容。 概述：

医疗机器人技术是集医学、生物力学、机械学、机械力学、材料学、计算机图形学、计算机视觉、数学分析、机器人等诸多学科为一体的新型交叉研究领域，已经成为国际机器人领域的一个研究热点。目前，利用微波消融手术（MA）治疗肝癌的研究方兴未艾，并已经取得了较好的研究成果，手术方案日趋成熟，在临床应用中也取得了很好的效果。不过现有的手术方案中仍存在不足之处，因此开展了机器人辅助介入性超声诊疗系统的研究。本文则重点分析该系统中采用的辅助机器人的正逆运动学分析。 分析：

该机器人的本体模型如下图所示。

图1 机器人本体模型

对于一个构型和各连杆长度已知的机械臂，由各个关节的角度，计算机械臂末端的位姿，称为正运动学分析。如果想要机械臂末端达到一个期望的位姿，就必须求得各关节运动的角度，这就叫做逆运动学分析。事实上，逆运动学分析更重要，因为机械臂的控制器正是根据逆运动学方程计算各个时刻每个关节的应达到的角度值，并把它作为位置给定，进行实时反馈控制的。

运动方程的数学基础：

机器人机构可以认为是由一系列关节连接起来的连杆机构组成。把构件坐标系嵌入机器人的每一个连杆机构中，可以方便的描述一个连杆与下一个连杆之间的关系。齐次变换是描述这些坐标系之间的相对位置和方向的一种通用方法，且把齐次变换记为A矩阵。一个A矩阵仅仅是描述连杆构件坐标系之间相对平移和旋转的齐次坐标变换。 构件坐标系的确定：

连杆串联型机器人是由一系列通过活动关节连接在一起的连杆组成。每个关节有一个自由度，或为回转副，或为移动副。对于具有n个关节（编号为1到n）的机器人，有n＋1个连杆（编号为0到n）。连杆0为机器人的基座，通常是固定的，连杆n则安装末端执行器，关节i连接连杆i和连杆i－1。

连杆可以被看作是定义两相邻关节轴间关系的一个刚体，由两个参数确定：连杆长度和连杆扭角。对第一个和最后一个连杆来说，连杆参数是没有意义的，通常选择为0。关节可以由两个参数描述，关节偏移是一个连杆与另一个连杆沿着关节轴线方向的距离。关节转角是一个连杆相对与下一个连杆沿关节轴线的旋转角度。

为了方便描述每一个连杆的位姿，我们将一个坐标系固定到连杆上，坐标系i与连杆i相连。Denavit和Hertenberg提出了为关节链中的杆件建立主附体坐标系的矩阵方法，即D-H法。有两种建立坐标系的D－H方法：标准形式和修正形式，标准形式将坐标系建立在连杆的下关节上，而修正形式将坐标系建立在连杆的上关节上。本课题中使用的是标准形式。如图2所示，关节i的轴线与zi?1方向相同，xi?1的方向平行于zi?1?zi并由zi?1指向zi，连杆与关节的参数可总结如下：

连杆长度 连杆扭角 连杆偏移 关节转角 ai zi?1与zi轴线沿xi轴的偏移距离 zi?1与zi轴线相对于xi轴的旋转角度 在zi?1轴上，坐标系i-1的原点到xi轴的距离 ?i di ?i xi?1轴与xi轴绕zi?1轴的旋转角度 对于旋转轴，关节变量是?i，di是常量，而对于棱柱关节，di是变量，?i是常量。

图2 两种形式的D-H坐标系设置

变换矩阵的建立：

一旦对全部的连杆坐标系之后，就可以按照下列的顺序由两个旋转和两个平移来建立相邻两连杆i-1和i之间的相对关系.

1) 绕zi?1轴旋转?i角，使xi?1轴转到与xi同一平面内。 2) 沿zi?1轴平移一距离di，把xi?1移到与xi同一直线上。

3) 沿i轴平移一距离ai?1，把连杆i-1的坐标系移到使其原点与连杆n的坐标系原点重

合的地方。

4) 绕xi?1轴旋转?i?1角，使zi?1转到与zi同一直线上。

这种关系可由表示连杆i对连杆i-1相对位置的四个齐次变换来描述,称为Ai矩阵.此关系式为

Ai?Rot(z,?i)Trans(0,0,di)Trans(ai,0,0)Rot(x,?i)

运动方程的表示：

A1表示第一个连杆相对于基坐标系的位置和姿态，A2表示第二个连杆相对于第一个连杆的位置和姿态，那么第二个连杆在基系中的位置和姿态可由下列矩阵的乘积给出：

T2＝A1A2

同理有：T3＝A1A2A3

称这些A矩阵的乘积为T矩阵，对于本课题中的5自由度机器人，有： T5＝A1A2A3A4A5

如果机械手与参考坐标系的相对关系是由变换Z来表示的，而且机械手与其端部工具的变换由变换D表示，那么此工具端部对参考坐标系的位置和方向可由变换X表示如下：

X＝ZT5D

坐标系的建立于D-H参数的确定：

根据D-H坐标系的建立方法，在系统的机构简图上建立各个杆件的坐标系，如图所示，其中坐标系0为机器人的基座标系，固定不动，坐标系1～5分别与杆件1～5（升降杆件、大臂、小臂、手腕连杆、穿刺工具）相固连。

需要说明的是，坐标系5与末端执行器固连，根据D-H坐标系定义方法，由于穿刺工具为末端杆件，坐标系5的位置可取在杆件的任意位置，但为简化计算，将其原点与坐标4相重合。

坐标系6同样与穿刺工具相固连，方向与坐标系5相同，原点为穿刺导槽所在的位置。

图3 机构简图

各坐标系间变换的D-H参数如下表所示： 相邻的两个坐标系 0-1 1-2 2-3 3-4 4-5 5-6 关节 变量 关节变量范围 0~280mm -55°~0° 95°~205° 45°~135° 45°~135° ?i 0° 0° 90° 90° -90° ai 0 a2（300mm） 0 0 0 di d1 0 0 d4（250mm） 0 d1(260) ) ?2(-31.16°) ?3(169.62°) ?4(90°) ?5(90°无 平移L1 L2 L3 L1=55mm L2= -29mm L3=70mm 正运动学分析：

机器人正运动学研究的内容是：给定机器人各关节的角度，计算机器人末端执行器相对于参考坐标系的位置和姿态问题。

根据上节所示的连杆参数，可计算得到各连杆的变换矩阵，机器人坐标系与参考坐标系为同一个坐标系，因此不计算矩阵Z，以下各式中使用了简化表示符号：

s1?sin(?1) c1?cos(?1) c12?cos(?1??2) s12?sin(?1??2)

?1?00?1?T1?Trans?0,0,d1????0??0010000100?0?? d1??1?

3． 求解关节变量?3

由①式得：S3?(qxC2?qyS2?a2)/d4

由②式得：C3?(qxS2?qyC2)/d4

所以：

?3?atan2(qxC2?qyS2?a2,qxS2?qyC2)

?2有两组解，将其值代入上式，即可得到对应的两组?3值

这样就得到了两组?2?3的解，分别对应着大臂小臂的两个位形：

图4 手臂的两种位形

本课题中，根据手术时机器人大小臂的位置，选取第二组解位形。 第二组解位形所对应的?2是两组解中较小的一个，因此有：

当atan2????22qx?qy?k2,k???0时，

?22?2?atan2(qy,qx)?atan2???qx?qy?k2,k??

??当atan2????22qx?qy?k2,k???0时，

?22?2?atan2(qy,qx)?atan2??qx?qy?k2,k??

??4． 求解关节变量?5

式3左右两边同时左乘2T32?1??3?得：

T3??3?1T2??2?0T1?1?1?1?d1?0T5?3T4??4?4T5??5? ?d1?0T5?3T5

0?0??d4??1?2T3??3?1T2??2?0T1?1?1?1??S3?0??C3??0C30S3001000??nxC2?nyS2oxC2?oyS2axC2?ayS2qxC2?qyS2?a2??C4C5?S4?C4S5??nS?nC?oS?oC?aS?aC?qS?qC??SCC?SS0?x2y2x2y2x2y2??45445???x2y2?0??nzozazqz?d1??S50C5????1??000100??0令两边矩阵的（2，1）与（2，3）元素相等，得：

S4C5?nz

?S4S5?az

当S4?0时有：

S5??az/S4 C5?nz/S4

从而有：

?5?atan2(?az,nz)

5． 求解关节变量?4

已知：S4C5?nz， 当C5?0时

S4?nz/C5

由式两边的（2，2）元素相等得：C4?oz 即可求得：?4?atan2(nz/C5,oz) 结论：

通过对此机器人的正逆运动学分析，使我更好的理解了坐标变换、机器人路径规划基础的相关知识，为今后的学习和科研工作打下坚实的基础。

本文来源：https://www.bwwdw.com/article/e8ig.html