用于具有三角形远心点机构的串联机械臂逆运动学数值解法
【技术领域】
[0001] 本发明设及一种串联机械臂逆运动学数值解法，具体设及一种用于具有Ξ角形远 屯、点机构的串联机械臂逆运动学数值解法，属于微创医疗器械领域。
【背景技术】
[0002] 微创手术与传统的开口手术相比具有手术切口小、术后瘤痕小、恢复时间快等优 点，因此微创手术被广泛地应用于临床手术。采用主从控制形式的微创手术机器人提高了 微创手术的精度和应用范围，故而有广阔的市场前景。微创手术机器人的机械臂通常采用 远屯、定点运动机构来实现手术器械在人体内的定位。Ξ角形远屯、机构依靠关节轴线相交于 定点的方式来实现机构的远屯、运动。前两个关节是转动关节，轴线相交于远屯、点，最后一个 关节是移动关节，轴线通过远屯、点。在远屯、机构后面串联腕部Ξ个关节，实现手术器械的运 动。在远屯、点机构前面可串联数个被动关节，用于手术前调整远屯、点的位置，手术中锁死。 机械臂逆运动学求解是根据机械臂在空间内的末端位置姿态矩阵，求解各个关节的位移 量。逆运动学求解是实现机械臂定位控制和轨迹跟踪控制的基础，且逆运动学算法直接影 响机械臂的运动控制器的性能。逆运动学方程是非线性超越方程，其求解过程非常复杂，而 且可能存在多解、无解或者奇异等问题。对于六自由度串联机械臂，仅当其结构满足准 则一-机械臂腕部Ξ个相邻的关节轴相交于一点，或者机械臂腕部Ξ个相邻的关节的轴线 相互平行，此时Ξ个相邻关节的位置和姿态是解禪的，可W求出机械臂逆运动学的封闭解。 但一般情况下，考虑到使用性能和设计制造和装配过程中的困难和误差，机械臂都不满足 运一准则。故而需要采用数值解法完成机械臂的逆运动学求解。一般采用的数值解法的基 本原理是根据机械臂当前的关节角度求解正运动学的计算结果，通过比较其与目标位置姿 态值的差值，对关节角度进行修正，反复试探并控制定位误差计算关节角度最优值。运样的 方法计算量大，不利于控制系统的实时性。
【发明内容】
[0003] 本发明为解决现有微创手术机械臂控制系统精度较低、实时性较差的问题，进而 提出具有=角形远屯、点机构的串联机械臂逆运动学数值解法。
[0004] 本发明为解决上述问题采取的技术方案是:本发明的具体步骤如下：
[0005] 步骤一、假设机械臂末端点与腕部起点〇4重合，即忽略手腕处Ξ个关节，运是假设 Α；
[0006] 步骤二、根据假设Α，输入量末端点位姿矩阵的位置分量与0〇坐标系下的远屯、点位 置矢量相减即为〇0坐标系下的第Ξ关节的位移量Q3的空间矢量，求出第Ξ关节位移量Q3的 中间计算值，命名为qi3;
[0007] 步骤Ξ、根据假设A，由移动第Ξ关节的轴线方向可W唯一地确定第一关节位移量 qi和第二关节位移量92的大小，依次求出第一关节中间计算值qii和第二关节中间计算值 qi2；
[0008] 步骤四、在不忽略第四关节、第五关节、第六关节的情况下，利用输入量末端点位 姿矩阵的姿态信息和根据假设A求出的第一关节中间计算值qii、第二关节中间计算值qi2、 第Ξ关节中间计算值qi3依次求出第四关节中间计算值qi4、第五关节中间计算值qi5、第六关 节中间计算值qi6;
[0009] 步骤五、假设第四关节真实值q〇4、第五关节真实值q〇5、第六关节真实值q〇6与步骤 四中计算出的第四关节中间计算值qi4、第五关节中间计算值qi5、第六关节中间计算值qi6相 等，运是假设B;
[0010] 步骤六、在第二关节坐标系化下，根据假设B，用第Ξ关节中间计算值qi3、第四关节 中间计算值qi4、第五关节中间计算值qi5、第六关节中间计算值qi6求出第Ξ关节新计算值 Q23；
[OOW 步骤屯、通过第；关节新计算值Q23和第四关节中间计算值qi4、第五关节中间计算 值qi5、第六关节中间计算值qi6依次求出第一关节新计算值Q21和第二关节新计算值q22;
[0012] 步骤八、通过第一关节新计算值Q21、第二关节新计算值Q22、第Ξ关节新计算值Q23 依次求出第四关节新计算值Q24、第五关节新计算值q25、第六关节新计算值Q26 ;
[0013] 步骤九、用第一关节新计算值Q21、第二关节新计算值Q22、第Ξ关节新计算值Q23、第 四关节新计算值024、第五关节新计算值025、第六关节新计算值Q26替代第一关节中间计算值 qii、第二关节中间计算值qi2、第Ξ关节中间计算值qi3、第四关节中间计算值qi4、第五关节中 间计算值qi5、第六关节中间计算值qi6，重新进入循环；
[0014] 步骤十、到达事先设定好的循环次数，推出循环，输出新的一组六个关节的位移。
[0015] 本发明的有益效果是：1、本发明求出的六个关节的位移量经过几次循环快速逼近 真值，经过机械臂正运动学计算得到的位置姿态矩阵的位置分量与输入的末端点位置姿态 矩阵的位置分量间的误差呈指数速度减小;2、位置姿态分离求解，先用位置信息，再用姿态 信息，每一次循环求出的一组解，可保证再正解出来的姿态矩阵没有误差，只有位置矢量有 误差。即在忽略其他误差的情况下，运用到机械臂上，末端位置有误差，姿态无误差;3、求解 过程在关节空间内完成，计算量小。
【附图说明】
[0016] 图1是本发明的算法流程图，图2是具有Ξ角形远屯、机构的六自由度串联机械臂的 坐标关系不意图，图3是由qi3、qi4、qi日、qi6求Q23的至间关系图，图2和图3中A1是第一关节轴 线，A2是第二关节轴线，A3是第Ξ关节轴线，A4是第四关节轴线，A5是第五关节轴线，A6是第 六关节轴线，qi是第一关节位移量，Q2是第二关节位移量，Q3是第Ξ关节位移量，Q4是第四关 节位移量，qs是第五关节位移量，qs是第六关节位移量，qoi第一关节真实值，q〇2是第二关节 真实值，q〇3是第Ξ关节真实值，q〇4是第四关节真实值，q〇5是第五关节真实值，q〇6是第六关节 真实值，qii是第一关节中间计算值，qi2是第二关节中间计算值，qi3是第立关节中间计算值， qi4是第四关节中间计算值，qi日是第五关节中间计算值，qi6是第六关节中间计算值，Q21是第 一关节新计算值，Q22是第二关节新计算值，Q23是跌Ξ关节新计算值，Q24是第四关节新计算 值，Q25是第五关节新计算值，Q26是第六关节新计算值，〇0是参考坐标系，〇1是第一关节坐标 系，〇2是第二关节坐标系，〇3是第Ξ关节坐标系，〇4是第四关节坐标系，〇5是第五关节坐标 系，〇6是第六关节坐标系，山第一关节杆件偏移量，d3是第Ξ关节杆件偏移量，LS是第五关节 杆件长度，L6是第六关节杆件长度，αι是第一关节杆件扭转角，02是第二关节杆件扭转角。
【具体实施方式】
[0017]【具体实施方式】一：结合图1至图3说明本实施方式，本实施方式所述用于具有Ξ角 形远屯、点机构的串联机械臂逆运动学数值解法的具体步骤如下：
[001引步骤一、假设机械臂末端点与腕部起点化重合，即忽略手腕处Ξ个关节，运是假设 Α；
[0019] 步骤二、根据假设Α，输入量末端点位姿矩阵的位置分量与0〇坐标系下的远屯、点位 置矢量相减即为〇0坐标系下的第Ξ关节的位移量Q3的空间矢量，求出第Ξ关节位移量Q3的 中间计算值，命名为qi3;
[0020] 步骤Ξ、根据假设A，由移动第Ξ关节的轴线方向可W唯一地确定第一关节位移量 qi和第二关节位移量Q2的大小，依次求出第一关节中间计算值qii和第二关节中间计算值 qi2；
[0021] 步骤四、在不忽略第四关节、第五关节、第六关节的情况下，利用输入量末端点位 姿矩阵的姿态信息和根据假设A求出的第一关节中间计算值qii、第二关节中间计算值qi2、 第Ξ关节中间计算值qi3依次求出第四关节中间计算值qi4、第五关节中间计算值qi5、第六关 节中间计算值qi6;
[0022] 步骤五、假设第四关节真实值q〇4、第五关节真实值q〇5、第六关节真实值q〇6与步骤 四中计算出的第四关节中间计算值qi4、第五关节中间计算值qi5、第六关节中间计算值qi6相 等，运是假设