法律状态公告日
法律状态信息
法律状态
2018-09-07
授权
授权
2016-06-29
实质审查的生效 IPC(主分类):G05B19/408 申请日:20151222
实质审查的生效
2016-06-01
公开
公开
技术领域
本发明涉及一种串联机械臂逆运动学数值解法,具体涉及一种用于具有三角形远心点 机构的串联机械臂逆运动学数值解法,属于微创医疗器械领域。
背景技术
微创手术与传统的开口手术相比具有手术切口小、术后疤痕小、恢复时间快等优点, 因此微创手术被广泛地应用于临床手术。采用主从控制形式的微创手术机器人提高了微创 手术的精度和应用范围,故而有广阔的市场前景。微创手术机器人的机械臂通常采用远心 定点运动机构来实现手术器械在人体内的定位。三角形远心机构依靠关节轴线相交于定点 的方式来实现机构的远心运动。前两个关节是转动关节,轴线相交于远心点,最后一个关 节是移动关节,轴线通过远心点。在远心机构后面串联腕部三个关节,实现手术器械的运 动。在远心点机构前面可串联数个被动关节,用于手术前调整远心点的位置,手术中锁死。 机械臂逆运动学求解是根据机械臂在空间内的末端位置姿态矩阵,求解各个关节的位移 量。逆运动学求解是实现机械臂定位控制和轨迹跟踪控制的基础,且逆运动学算法直接影 响机械臂的运动控制器的性能。逆运动学方程是非线性超越方程,其求解过程非常复杂, 而且可能存在多解、无解或者奇异等问题。对于六自由度串联机械臂,仅当其结构满足准 则——机械臂腕部三个相邻的关节轴相交于一点,或者机械臂腕部三个相邻的关节的轴线 相互平行,此时三个相邻关节的位置和姿态是解耦的,可以求出机械臂逆运动学的封闭解。 但一般情况下,考虑到使用性能和设计制造和装配过程中的困难和误差,机械臂都不满足 这一准则。故而需要釆用数值解法完成机械臂的逆运动学求解。一般采用的数值解法的基 本原理是根据机械臂当前的关节角度求解正运动学的计算结果,通过比较其与目标位置姿 态值的差值,对关节角度进行修正,反复试探并控制定位误差计算关节角度最优值。这样 的方法计算量大,不利于控制系统的实时性。
发明内容
本发明为解决现有微创手术机械臂控制系统精度较低、实时性较差的问题,进而提出 具有三角形远心点机构的串联机械臂逆运动学数值解法。
本发明为解决上述问题采取的技术方案是:本发明的具体步骤如下:
步骤一、假设机械臂末端点与腕部起点O4重合,即忽略手腕处三个关节,这是假设 A;
步骤二、根据假设A,输入量末端点位姿矩阵的位置分量与O0坐标系下的远心点位 置矢量相减即为O0坐标系下的第三关节的位移量q3的空间矢量,求出第三关节位移量q3的中间计算值,命名为q13;
步骤三、根据假设A,由移动第三关节的轴线方向可以唯一地确定第一关节位移量q1和第二关节位移量q2的大小,依次求出第一关节中间计算值q11和第二关节中间计算值 q12;
步骤四、在不忽略第四关节、第五关节、第六关节的情况下,利用输入量末端点位姿 矩阵的姿态信息和根据假设A求出的第一关节中间计算值q11、第二关节中间计算值q12、 第三关节中间计算值q13依次求出第四关节中间计算值q14、第五关节中间计算值q15、第 六关节中间计算值q16;
步骤五、假设第四关节真实值q04、第五关节真实值q05、第六关节真实值q06与步骤 四中计算出的第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16相等,这是假设B;
步骤六、在第二关节坐标系O2下,根据假设B,用第三关节中间计算值q13、第四关 节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16求出第三关节新计 算值q23;
步骤七、通过第三关节新计算值q23和第四关节中间计算值q14、第五关节中间计算值 q15、第六关节中间计算值q16依次求出第一关节新计算值q21和第二关节新计算值q22;
步骤八、通过第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23依 次求出第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26;
步骤九、用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23、 第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26替代第一关节中间 计算值q11、第二关节中间计算值q12、第三关节中间计算值q13、第四关节中间计算值q14、 第五关节中间计算值q15、第六关节中间计算值q16,重新进入循环;
步骤十、到达事先设定好的循环次数,推出循环,输出新的一组六个关节的位移。
本发明的有益效果是:1、本发明求出的六个关节的位移量经过几次循环快速逼近真 值,经过机械臂正运动学计算得到的位置姿态矩阵的位置分量与输入的末端点位置姿态矩 阵的位置分量间的误差呈指数速度减小;2、位置姿态分离求解,先用位置信息,再用姿 态信息,每一次循环求出的一组解,可保证再正解出来的姿态矩阵没有误差,只有位置矢 量有误差。即在忽略其他误差的情况下,运用到机械臂上,末端位置有误差,姿态无误差; 3、求解过程在关节空间内完成,计算量小。
附图说明
图1是本发明的算法流程图,图2是具有三角形远心机构的六自由度串联机械臂的坐标关 系示意图,图3是由q13、q14、q15、q16求q23的空间关系图,图2和图3中A1是第一关 节轴线,A2是第二关节轴线,A3是第三关节轴线,A4是第四关节轴线,A5是第五关节 轴线,A6是第六关节轴线,q1是第一关节位移量,q2是第二关节位移量,q3是第三关节 位移量,q4是第四关节位移量,q5是第五关节位移量,q6是第六关节位移量,q01第一关 节真实值,q02是第二关节真实值,q03是第三关节真实值,q04是第四关节真实值,q05是 第五关节真实值,q06是第六关节真实值,q11是第一关节中间计算值,q12是第二关节中 间计算值,q13是第三关节中间计算值,q14是第四关节中间计算值,q15是第五关节中间计 算值,q16是第六关节中间计算值,q21是第一关节新计算值,q22是第二关节新计算值,q23是跌三关节新计算值,q24是第四关节新计算值,q25是第五关节新计算值,q26是第六关 节新计算值,O0是参考坐标系,O1是第一关节坐标系,O2是第二关节坐标系,O3是第三 关节坐标系,O4是第四关节坐标系,O5是第五关节坐标系,O6是第六关节坐标系,d1第 一关节杆件偏移量,d3是第三关节杆件偏移量,L5是第五关节杆件长度,L6是第六关节 杆件长度,α1是第一关节杆件扭转角,α2是第二关节杆件扭转角。
具体实施方式
具体实施方式一:结合图1至图3说明本实施方式,本实施方式所述用于具有三角形 远心点机构的串联机械臂逆运动学数值解法的具体步骤如下:
步骤一、假设机械臂末端点与腕部起点O4重合,即忽略手腕处三个关节,这是假设 A;
步骤二、根据假设A,输入量末端点位姿矩阵的位置分量与O0坐标系下的远心点位 置矢量相减即为O0坐标系下的第三关节的位移量q3的空间矢量,求出第三关节位移量q3的中间计算值,命名为q13;
步骤三、根据假设A,由移动第三关节的轴线方向可以唯一地确定第一关节位移量q1和第二关节位移量q2的大小,依次求出第一关节中间计算值q11和第二关节中间计算值 q12;
步骤四、在不忽略第四关节、第五关节、第六关节的情况下,利用输入量末端点位姿 矩阵的姿态信息和根据假设A求出的第一关节中间计算值q11、第二关节中间计算值q12、 第三关节中间计算值q13依次求出第四关节中间计算值q14、第五关节中间计算值q15、第 六关节中间计算值q16;
步骤五、假设第四关节真实值q04、第五关节真实值q05、第六关节真实值q06与步骤 四中计算出的第四关节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16相等,这是假设B;
步骤六、在第二关节坐标系O2下,根据假设B,用第三关节中间计算值q13、第四关 节中间计算值q14、第五关节中间计算值q15、第六关节中间计算值q16求出第三关节新计 算值q23;
步骤七、通过第三关节新计算值q23和第四关节中间计算值q14、第五关节中间计算值 q15、第六关节中间计算值q16依次求出第一关节新计算值q21和第二关节新计算值q22;
步骤八、通过第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23依 次求出第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26;
步骤九、用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23、 第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26替代第一关节中间 计算值q11、第二关节中间计算值q12、第三关节中间计算值q13、第四关节中间计算值q14、 第五关节中间计算值q15、第六关节中间计算值q16,重新进入循环;
步骤十、到达事先设定好的循环次数,推出循环,输出新的一组六个关节的位移值。
本实施方式中第二关节位移量q2是通过公式(1)得出的:
公式(1)中表示参考坐标系下第三关节坐标系的齐次变换矩阵,表示参考坐 标系下第一关节坐标系的齐次变换矩阵,表示第一关节坐标系下第二关节坐标系的齐 次变换矩阵,表示第二关节坐标系下第三关节坐标系的齐次变换矩阵,表示参考坐 标系下第三关节坐标系的齐次变换矩阵的姿态矩阵分量,表示参考坐标系下第三关节 坐标系的齐次变换矩阵的位置矢量分量,表示算法输入量末端点位置姿态矩阵的姿态 矩阵分量,表示算法输入量末端点位置姿态矩阵的位置矢量分量,表示算法输入量 末端点位置姿态矩阵;根据假设A,与,与相等。
公式(1)中等式两边位置分量各对应项相等,寻找较简单的表达式,得到公式(2)。
公式(2)中表示算法输入量末端点位置姿态矩阵的位置分量的Z方向值,α1表 示第一关节杆件扭转角,α2表示第二关节杆件扭转角,d1表示第一关节杆件偏移量;
应当注意,因为需要求反三角函数,所以为了尽量消除多解,应考虑到关节角在使用 过程中的运动范围,合理选择具体采用哪一对对应项进行求解。
再求第一关节中间计算值q11。对公式(1),等式两边左乘得到公式(3):
公式(3)中表示矩阵的位置分量,表示的逆矩阵,依旧使等 式两边对应量相等,求第一关节中间计算值q11。这里为了计算简便,可将两个对应项方 程联立求出sinq11或者cosq11。
步骤二和步骤三利用输入矩阵的位置分量的三个值求出了假设条件下的第一关节位 移量q1、第二关节位移量q2和第三关节位移量q3的值,即第一关节中间计算值q11、第二 关节中间计算值q12和第三关节中间计算值q13。
步骤四是在不忽略第四关节、第五关节、第六关节的情况下,利用输入量末端点位姿 矩阵的姿态信息和根据假设A求出的第一关节中间计算值q11、第二关节中间计算值q12、 第三关节中间计算值q13,依次求出第五关节中间计算值q15、第六关节中间计算值q16、 第四关节中间计算值q14。所以步骤四中只计算矩阵的姿态部分。
本实施方式步骤四中第五关节中间计算值q15是通过公式(4)、(5)得出的:
-cosq15=az(5),
公式(4)中表示第三关节坐标系下第四关节坐标系的齐次变换矩阵,表示第 四关节坐标系下第五关节坐标系的齐次变换矩阵,表示第五关节坐标系下第六关节坐 标系的齐次变换矩阵,表示的逆矩阵。
对公式(4),选择较简单的对应项相等,得到公式(5)。
公式(5)中az表示等式右侧矩阵对应项。
再求第六关节中间计算值q16。对公式(4),选择较简单的对应项相等,得到公式(6)。
-sinq15·sinq16=oz(6),
公式(6)中oz表示等式右侧矩阵对应项。
再求第四关节中间计算值q14。对公式(4),选择较简单的对应项相等,得到公式(7)。
sinq14·sinq15=ay(7),
公式(7)中ay表示等式右侧矩阵对应项。
因第一关节中间计算值q11、第二关节中间计算值q12和第三关节中间计算值q13是在假 设条件下求出的,存在误差,所以第四关节中间计算值q14、第五关节中间计算值q15和第 六关节中间计算值q16也存在误差。其中第三关节中间计算值q13比真值大,大体体现出真 实腕部位置和真实末端位置的差。而从第一关节中间计算值q11、第二关节中间计算值q12求解的表达式来看,因为是两个比真值大的数相除,所以误差相对较小。第四关节中间计 算值q14、第五关节中间计算值q15、第六关节中间计算值q16的情况类似。而且,因为存 在远心点机构,所以第一关节真实值q01、第二关节真实值q02和上面求得的第一关节中间 计算值q11、第二关节中间计算值q12都使得坐标系原点O2处在远心点上。而第三关节的轴 线A3的真实方向与第一关节中间计算值q11和第二关节中间计算值q12作用下的A3方向不 同。根据上述分析,现在需要将求得的第三关节中间计算值q13的大小尽量接近真值。
这里进入循环。因为第四关节中间计算值q14、第五关节中间计算值q15、第六关节中 间计算值q16与真值的误差相对较小,为了使第三关节中间计算值q13尽量接近真值,第三 关节中间计算值q13的轴线方向尽量接近真值方向,则假设第四关节真实值q04、第五关节 真实值q05、第六关节真实值q06与刚计算出的第四关节中间计算值q14、第五关节中间计 算值q15、第六关节中间计算值q16相等。此为假设B。
步骤六是在第二关节的坐标系O2下(坐标系O2的原点即为远心点),根据假设B,用 第三关节中间计算值q13、第四关节中间计算值q14、第五关节中间计算值q15、第六关节 中间计算值q16,求出第三关节新计算值q23。
由附图3所示:根据假设B,将第三关节新计算值q23、第四关节中间计算值q14、第 五关节中间计算值q15和第六关节中间计算值q16代入从第二关节坐标系到第六关节坐标 系的运动学正解,可求得机械臂末端在第二关节坐标系O2下的位置。根据假设B,该位 置可认为是机械臂末端的真实位置。而机械臂末端位置到第二关节坐标系原点(即远心点) O2的距离即为q13。
上述过程用矩阵形式表示为:
公式(8)中表示代入第三关节新计算值q23的第二关节坐标系下的第三关节坐标 系的齐次变换矩阵,表示代入第三关节新计算值q23的第二关节坐标系下的第六关节坐 标系的齐次变换矩阵的姿态矩阵,表示代入第三关节新计算值q23的第二关节坐标系下 的第六关节坐标系的齐次变换矩阵的位置矢量,表示在第二关节坐标系X轴上的 分量,表示在第二关节坐标系Y轴上的分量,表示在第二关节坐标系Z轴 上的分量。
对公式(8)展开得到公式(10):
将公式(10)代入公式(9),求出第三关节新计算值q23。
将第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23代入齐次变 换方程,得到公式(11):
公式(11)中表示代入第三关节新计算值q23的参考坐标系下的第一关节坐标系的 齐次变换矩阵、表示代入第三关节新计算值q23的第一关节坐标系下的第二关节坐标系 的齐次变换矩阵。
将公式(11)中的右乘到方程右边去,得到公式(12):
对公式(12),矩阵位置分量对应项相等,选择较简单的一对,得到公式(13)
d1+q23·(cosα1·cosα2-cosq22·sinα1·sinα2)=Pz'(13),
公式(13)中,Pz'表示等式右侧对应项,从而求出第二关节的新计算值q22。
再求第一关节的新计算值q21。将公式(12)中的左乘过去,得到公式(14):
依旧按照矩阵对应项相等,求出第一关节的新计算值q21。这里为了计算简便,可将 两个对应项方程联立,求出sinq21或者cosq21。
求出第一关节的新计算值q21和第二关节的新计算值q22,使得第三关节的轴线A3方 向也变得比较准。
步骤八是利用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23依 次求出第五关节新计算值q25、第六关节新计算值q26、第四关节新计算值q24;
抛弃假设B。对公式(11),将第四关节新计算值q24、第五关节新计算值q25、第六 关节新计算值q26代入,得到公式(15):
公式(15)中,表示代入第四关节新计算值q24的第三关节坐标系下的第四关节坐 标系的齐次变换矩阵,表示代入第五关节新计算值q25的第四关节坐标系下的第五关节 坐标系的齐次变换矩阵,表示代入第六关节新计算值q26的第五关节坐标系下的第六关 节坐标系的齐次变换矩阵。
将左乘到方程右侧,得到公式(16):
公式(16)中,表示的逆矩阵,表示的逆矩阵,表示的逆矩阵。
与之前的过程类似,对公式(16),矩阵的姿态分量各对应项相等,寻找较简单的一 对,得到公式(17):
-cosq25=az′(17),
公式(17)中az′表示等式右侧矩阵对应项。求出第五关节新计算值q25。
再求第六关节新计算值q26。对公式(16),选择较简单的对应项相等,得到公式(18)。
-sinq25·sinq26=oz'(18),
公式(18)中oz′表示等式右侧矩阵对应项。求出第六关节新计算值q26。
再求第四关节新计算值q24。对公式(16),选择较简单的对应项相等,得到公式(19)。
sinq24·sinq25=ay'(19),
公式(19)中ay′表示等式右侧矩阵对应项。求出第四关节新计算值q24。
至此,就求出了新的一组比较准的解第一关节新计算值q21、第二关节新计算值q22、 第三关节新计算值q23、第四关节新计算值q24、第五关节新计算值q25、第六关节新计算 值q26。新的一组解比旧的一组解精度更高。
步骤九是用第一关节新计算值q21、第二关节新计算值q22、第三关节新计算值q23、 第四关节新计算值q24、第五关节新计算值q25、第六关节新计算值q26替代第一关节中间 计算值q11、第二关节中间计算值q12、第三关节中间计算值q13、第四关节中间计算值q14、 第五关节中间计算值q15、第六关节中间计算值q16,重新进入循环。这样不断地用算出的 结果作为循环的输入,从而迭代得出误差更小的解。
需要强调的是,因为第四关节新计算值q24、第五关节新计算值q25、第六关节新计算 值q26是根据算法输入的姿态矩阵的对应项相等求出的,所以求出的一组解根据正运动学 计算得到的姿态是正确的,只有位置是有误差的。
以上所述,仅是本发明的较佳实施例而已,并非对本发明作任何形式上的限制,虽然 本发明已以较佳实施例揭露如上,然而并非用以限定本发明,任何熟悉本专业的技术人员, 在不脱离本发明技术方案范围内,当可利用上述揭示的技术内容做出些许更动或修饰为等 同变化的等效实施例,但凡是未脱离本发明技术方案内容,依据本发明的技术实质,在本 发明的精神和原则之内,对以上实施例所作的任何简单的修改、等同替换与改进等,均仍 属于本发明技术方案的保护范围之内。
机译: 用于多关节机构的逆运动学解决方案,以及使用逆运动学解决方案的教学数据创建设备
机译: 用于多关节机构的逆运动学解决方案,以及使用逆运动学解决方案来创建指令数据的装置
机译: 用于多关节机构的逆运动学解决方案,以及通过使用逆运动学解决方案来创建指令数据的装置