CN109434838A - 线驱动连续机器人内窥操作的协同运动规划方法及系统 - Google Patents
线驱动连续机器人内窥操作的协同运动规划方法及系统 Download PDFInfo
- Publication number
- CN109434838A CN109434838A CN201811587658.1A CN201811587658A CN109434838A CN 109434838 A CN109434838 A CN 109434838A CN 201811587658 A CN201811587658 A CN 201811587658A CN 109434838 A CN109434838 A CN 109434838A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- joint space
- space
- continuous
- differential motion
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 229920001971 elastomer Polymers 0.000 claims description 36
- 239000000806 elastomer Substances 0.000 claims description 36
- 239000011159 matrix material Substances 0.000 claims description 26
- 238000004364 calculation method Methods 0.000 claims description 11
- 239000013598 vector Substances 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 4
- 238000005452 bending Methods 0.000 description 3
- 238000009795 derivation Methods 0.000 description 3
- 238000002324 minimally invasive surgery Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000000474 nursing effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明公开了线驱动连续机器人内窥操作的协同运动规划方法及系统。所述方法包括:获取线驱动连续机器人的连续机械臂;将连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂;获取操作空间预设运动路径和关节空间预设运动路径;计算第一虚拟机械臂在操作空间预设运动路径条件下的关节空间微分运动规律以及第二虚拟机械臂在关节空间预设运动路径条件下的关节空间微分运动规律;计算整个连续机械臂在位形空间的微分运动规律,连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。本发明能在操作空间和位形空间同时进行协同运动规划,能避免位形漂移引起的意外接触或碰撞,安全性高,且工作效率高。
Description
技术领域
本发明涉及机器人技术领域,特别是涉及线驱动连续机器人内窥操作的协同运动规划方法及系统。
背景技术
线驱动连续机器人由多段弹性体组成,其在微创手术、内窥检测、康复护理、人机交互操作、人机协同操作等领域具有广泛应用前景。多段弹性体组成的线驱动连续机器人具有以下特点:(1)连续机器人机构结构刚度很低,受力产生的非线性变形大,精确建立系统的运动学和动力学模型困难;(2)基于粗略模型实现连续机器人的精确运动和力操作控制具有相当的难度;(3)连续机器人输出力和能量的能力十分有限,实现大范围力操作任务不易实现;(4)为提高连续机器人的操作精度,这类机器人通常需要采用大量独立驱动输入,机器人整体系统的控制输入和状态变量很多,实时控制存在一定难度;(5)相对于机器人的操作任务需求,这类机器人通常具有超冗余自由度的特点。由于线驱动连续机器人具有上述特点,使得机器人的逆运动学解不封闭,在机器人执行多次重复操作过程中,机器人的关节空间位形会产生漂移,而这种位形漂移会导致机器人完成末端操作任务时,机械臂在位形空间与环境发生意外接触或碰撞,导致操作任务失败。目前,为了避免上述情况发生,通常需要定时定期进行关节空间位形的重新标定,这样就大幅降低了机器人的工作效率。
发明内容
基于此,有必要提供线驱动连续机器人内窥操作的协同运动规划方法及系统,在操作空间和位形空间同时进行协同运动规划,以避免多段弹性体连续机器人的超冗余自由度机器人系统,以及运动学逆解不封闭引起的机器人机械臂运动控制漂移问题,实现多段弹性体连续机器人机械臂安全内窥操作,且提高工作效率。
为实现上述目的,本发明提供了如下方案:
线驱动连续机器人内窥操作的协同运动规划方法,包括:
获取线驱动连续机器人的连续机械臂;所述连续机械臂包括m段弹性体;
将所述连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂;所述第一虚拟机械臂由所述连续机械臂中从基础端开始的连续s段的弹性体组成;所述第二虚拟机械臂由所述连续机械臂中从s+1段开始的连续m-s段的弹性体组成;m、s均为正整数,且m>s;
获取所述第一虚拟机械臂的操作空间预设运动路径和所述第二虚拟机械臂的关节空间预设运动路径;
依据所述操作空间预设运动路径,计算第一关节空间微分运动规律;所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律;
依据所述关节空间预设运动路径,计算第二关节空间微分运动规律;所述第二关节空间微分运动规律为所述第二虚拟机械臂在所述关节空间预设运动路径条件下的关节空间微分运动规律;
依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律,得到所述连续机械臂在位形空间的微分运动规律;所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。
可选的,所述依据所述操作空间预设运动路径,计算第一关节空间微分运动规律,具体包括:
依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
其中,表示操作空间预设运动路径P的微分运动规律,表示第一虚拟机械臂的s段弹性体的雅克比矩阵,表示关节空间的微分运动规律;
对所述运动学方程求逆解,得到第一关节空间微分运动规律所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律。
可选的,所述依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律,得到所述连续机械臂在位形空间的微分运动规律,具体为:
其中,表示连续机械臂在位形空间的微分运动规律,表示第一关节空间微分运动规律,表示第二关节空间微分运动规律,P表示对于第一虚拟机械臂的操作空间预设运动路径,Q表示对于第二虚拟机械臂的关节空间预设运动路径。
可选的,所述第一虚拟机械臂的s段弹性体的雅克比矩阵具体为:
其中,表示伴随矩阵,表示第i-1段弹性体基础端面的坐标系相对于机器人基础坐标系的旋转矢量,表示第i-1段弹性体基础端面的坐标系的原点在机器人基础坐标系中的位置矢量;
其中,表示第i-1段弹性体自由端面的坐标系到第i-1段弹性体基础端面的坐标系之间的坐标变换矩阵,Θi-1表示第i-1段弹性体在位形空间中的运动学变量,φi-1表示第i-1段弹性体自由端面的坐标系在基础端面的坐标系中的旋转角,θi-1表示第i-1段弹性体自由端面与基础端面之间的二面角,κi-1表示第i-1段弹性体的形状曲率,li-1表示第i-1段弹性体的长度,i=1,2,...,m。
可选的,所述第一虚拟机械臂的空间维数与所述操作空间预设运动路径的空间维数相同。
本发明还提供了线驱动连续机器人内窥操作的协同运动规划系统,所述系统包括:
模型获取模块,用于获取线驱动连续机器人的连续机械臂;所述连续机械臂包括m段弹性体;
划分模块,用于将所述连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂;所述第一虚拟机械臂由所述连续机械臂中从基础端开始的连续s段的弹性体组成;所述第二虚拟机械臂由所述连续机械臂中从s+1段开始的连续m-s段的弹性体组成;m、s均为正整数,且m>s;
路径获取模块,用于获取所述第一虚拟机械臂的操作空间预设运动路径和所述第二虚拟机械臂的关节空间预设运动路径;
第一计算模块,用于依据所述操作空间预设运动路径,计算第一关节空间微分运动规律;所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律;
第二计算模块,用于依据所述关节空间预设运动路径,计算第二关节空间微分运动规律;所述第二关节空间微分运动规律为所述第二虚拟机械臂在所述关节空间预设运动路径条件下的关节空间微分运动规律;
第三计算模块,用于依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律,得到所述连续机械臂在位形空间的微分运动规律;所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。
可选的,所述第一计算模块,具体包括:
方程建立单元,用于依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
其中,表示操作空间预设运动路径P的微分运动规律,表示第一虚拟机械臂的s段弹性体的雅克比矩阵,表示关节空间的微分运动规律;
逆解计算单元,用于对所述运动学方程求逆解,得到第一关节空间微分运动规律所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律。
可选的,所述第三计算模块,具体为:
其中,表示连续机械臂在位形空间的微分运动规律,表示第一关节空间微分运动规律,表示第二关节空间微分运动规律。
与现有技术相比,本发明的有益效果是:
本发明提出了线驱动连续机器人内窥操作的协同运动规划方法及系统。所述协同运动规划方法中将连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂;获取操作空间预设运动路径和关节空间预设运动路径;计算第一虚拟机械臂在操作空间预设运动路径条件下的关节空间微分运动规律以及第二虚拟机械臂在关节空间预设运动路径条件下的关节空间微分运动规律;计算连续机械臂在位形空间的微分运动规律,连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。采用本发明的方法或系统,能够在操作空间和位形空间同时进行协同运动规划,避免多段弹性体连续机器人的超冗余自由度机器人系统,以及运动学逆解不封闭引起的机器人机械臂运动控制漂移问题,实现多段弹性体连续机器人机械臂安全内窥操作,安全性高;并且避免了定时定期进行关节空间位形的重新标定,提高了机器人的工作效率。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例线驱动连续机器人内窥操作的协同运动规划方法的流程图;
图2为本发明实施例线驱动连续机器人内窥操作的协同运动规划方法的原理示意图;
图3为本发明实施例线驱动连续机器人的几何模型图;
图4为本发明六段弹性体连续机器人的内窥操作运动规划仿真结果图;
图5为本发明实施例线驱动连续机器人内窥操作的协同运动规划系统的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本发明作进一步详细的说明。
多段弹性体连续机器人的重要应用之一是在狭窄空间环境中完成操作,例如微创手术中的腹腔镜操作,或者发动机复杂腔体结构的内窥检测操作。对于一般开阔空间中操作的刚性冗余自由度机械臂,可以方便地采用机器人的运动学逆解实现操作任务,并利用逆解的零空间运动实现机械臂位形空间运动的最优化。但是对多段弹性体连续机器人这种超冗余自由度机械臂系统,其运动学逆解空间维数过大,逆解在零空间中的很小摄动运动将引起机械臂关节空间运动的漂移,使得连续机器人在完成内窥操作过程中,可能与环境空间结构发生摩擦碰撞,造成操作任务失败。
为了实现连续机器人在复杂狭窄空间中的安全内窥操作,本实施例提出一种在操作空间和关节空间同时进行运动规划的协同方法。图1为本发明实施例线驱动连续机器人内窥操作的协同运动规划方法的流程图,图2为本发明实施例线驱动连续机器人内窥操作的协同运动规划方法的原理示意图。
参见图1-2,实施例的线驱动连续机器人内窥操作的协同运动规划方法,包括:
步骤S1:获取线驱动连续机器人的连续机械臂。
所述连续机械臂包括m段弹性体。
步骤S2:将所述连续机械臂划分为第一虚拟机械臂Aos和第二虚拟机械臂Asm。
所述第一虚拟机械臂Aos由所述连续机械臂中从基础端开始的连续s段的弹性体组成;所述第二虚拟机械臂Asm由所述连续机械臂中从s+1段开始的连续m-s段的弹性体组成;m、s均为正整数,且m>s。
步骤S3:获取所述第一虚拟机械臂Aos的操作空间预设运动路径P和所述第二虚拟机械臂Asm的关节空间预设运动路径Q。所述操作空间预设运动路径P是采用操作空间运动规划方法得到的,所述关节空间预设运动路径Q是采用关节空间运动规划方法得到的。
步骤S4:依据所述操作空间预设运动路径P,计算第一关节空间微分运动规律。
所述第一关节空间微分运动规律为所述第一虚拟机械臂Aos在所述操作空间预设运动路径P条件下的关节空间微分运动规律。
所述步骤S4具体包括:
(1)依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
其中,表示操作空间预设运动路径P的微分运动规律,表示第一虚拟机械臂的s段弹性体的雅克比矩阵,表示关节空间的微分运动规律。
所述第一虚拟机械臂的s段弹性体的雅克比矩阵具体为:
其中,表示伴随矩阵,表示第i-1段弹性体基础端面的坐标系相对于机器人基础坐标系的旋转矢量,表示第i-1段弹性体基础端面的坐标系的原点在机器人基础坐标系中的位置矢量;
其中,表示第i-1段弹性体自由端面的坐标系到第i-1段弹性体基础端面的坐标系之间的坐标变换矩阵,Θi-1表示第i-1段弹性体在位形空间中的运动学变量,φi-1表示第i-1段弹性体自由端面的坐标系在基础端面的坐标系中的旋转角,θi-1表示第i-1段弹性体自由端面与基础端面之间的二面角,κi-1表示第i-1段弹性体的形状曲率,li-1表示第i-1段弹性体的长度,i=1,2,...,m。
(2)对所述运动学方程求逆解,得到第一关节空间微分运动规律所述第一关节空间微分运动规律为所述第一虚拟机械臂Aos在所述操作空间预设运动路径P条件下的关节空间微分运动规律。具体求解过程为:
上述步骤(1)中涉及的雅克比矩阵具体推导过程如下,下面以m段弹性体组成的线驱动连续机器人为例进行推导,图3为本发明实施例线驱动连续机器人的几何模型图。
参见图3,该图表示任意一段弹性体的几何模型图,∑i-1(oi-1-xi-1yi-1zi-1)表示弹性体的基础坐标系,并与弹性体的基础端面的中心固联;∑i(oi-xiyizi)为弹性体自由端面固联坐标系;坐标轴zi-1、zi与弹性体的中心龙骨曲线的切线重合;θi-1表示弹性体基础端面与自由端面之间的二面角;φi-1表示坐标系∑i在坐标系∑i-1中的旋转角。因此,在弹性体的位形空间中,其运动学变量可表示为Θi-1=[κi-1φi-1li-1]T,κi-1表示弹性段oi-1-oi的形状曲率,本实施例中,假设每段弹性体的弯曲变形曲率是均匀的。基于以上符号定义,弹性体的运动可看做由绕轴zi-1的相对旋转角θi-1和弹性体的面内弯曲运动组成。
采用螺旋坐标
描述弹性体的运动,其中和分别表示弹性体的平移运动向量和旋转运动向量,ξrot表示绕z轴的旋转量,ξinp表示绕y轴的弯曲。借助于楔算子“∧”,ξrot和ξinp所示的螺旋坐标可表示为两个旋量
其中,为 为
进一步应用指数积公式,从坐标系∑i到坐标系∑i-1的坐标变换矩阵可表示为
其中,表示坐标系∑i相对于坐标系∑i-1的旋转矩阵,表示坐标系∑i的原点在坐标系∑i-1中的位置矢量,
对于由m段弹性体组成的连续机器人,其末端操作器中心坐标系到机器人基础坐标系之间的坐标变换矩阵可表示为
再次应用指数积公式,从上式的时间导数可得到机械臂末端操作器的速度
其中表示速度矢量的旋量,上式的矩阵形式可表示为
其中为连续机器人机械臂的运动学雅克比矩阵,且可以明确给出为
其中“∨”表示楔算子“∧”的逆算子。
利用伴随变换
雅克比矩阵可进一步表示为更明确形式
其中,
由上述推导,得到了最终的连续机器人运动学雅克比矩阵,该运动学雅克比矩阵基于每段弹性体弯曲曲率均匀的假设。在弹性体重量较轻,机械臂受到外力较小的条件下,上述最终的连续机器人运动学雅克比矩阵的准确性能够满足实际应用。
步骤S5:依据所述关节空间预设运动路径Q,计算第二关节空间微分运动规律。所述第二关节空间微分运动规律为所述第二虚拟机械臂Asm在所述关节空间预设运动路径Q条件下的关节空间微分运动规律。
具体的,所述第二关节空间微分运动规律可通过对所述关节空间预设运动路径Q求导得到。
步骤S6:依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律,得到所述连续机械臂在位形空间的微分运动规律。
所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。
所述步骤S6具体为:
其中,表示连续机械臂在位形空间的微分运动规律,表示第一关节空间微分运动规律,表示第二关节空间微分运动规律,P表示操作空间预设运动路径,Q表示关节空间预设运动路径。
本实施例在划分连续机械臂时,使得所述第一虚拟机械臂Aos的空间维数与所述操作空间预设运动路径P的空间维数相同。这样就保证了第一虚拟机械臂Aos是非冗余自由度机械臂,从而消除了第一虚拟机械臂Aos的逆解的零空间运动,保证了第一虚拟机械臂Aos逆解的准确性。
本实施例的第二虚拟机械臂Asm需要工作在狭窄且连通的空间(如图2中的Apart)中。在该狭窄且连通的工作空间中,总存在一个截面半径为r的虚拟光滑管道空间Qtube。当该虚拟光滑管道的截面半径rtube大于连续机器人的截面半径rrob和机器人每段弹性体的长度lseg,且该光滑管道的最大曲率κtube小于连续机械臂弹性段的可达最大曲率,则第二虚拟机械臂Asm的位形空间是局部充分可达的。基于以上条件,对于任意给定的基础段虚拟机械臂位置pi∈P(i=1,2…,n),总存在第二虚拟机械臂Asm的局部可达位形(κi,φi),(i∈[s+1,m])与虚拟管道Qtube的空间形状(κtube,φtube)是一致的,因此,通过局部逼近虚拟管道空间形状,可获得第二虚拟机械臂Asm在位形空间中的微分运动规律,即第二关节空间微分运动规律
下面本实施例提供的线驱动连续机器人内窥操作的协同运动规划方法的有效性进行了验证。
对于6段弹性体连续机器人机械臂,设机械臂的弹性段长度lseg=140mm,每个弹性体的最大可达曲率为κm=2π/L。现有一复杂结构箱体,其外形尺寸如图4所示,且该复杂结构箱体中虚拟光滑管道的曲率满足条件κ(l)<π/L和φtube=0。现将机械臂分为两部分,基础部分虚拟机械臂(第一虚拟机械臂)Aos包含起始的3段弹性体(s=3),末段部分虚拟机械臂(第二虚拟机械臂)Asm包含剩余的3段弹性体。对于给定的基础段虚拟机械臂运动路径Pd(k)(1,2,…,N),设探入虚拟管道的机械臂长度为l(k),则末段部分虚拟机械臂Asm在内窥操作中的目标位形曲率变量可写出为
给定内窥操作时基础段虚拟机械臂的末端移动速度为采用上述实施例给出的协同运动规划方法,可计算获得六段弹性体连续机器人的内窥操作运动规划仿真结果图,如图4所示,图4中横坐标表示世界坐标系的x轴,纵坐标表示世界坐标系的z轴,单位均为mm。由图4可知,连续机器人成功探入复杂腔体结构内部而未与箱体内壁发生接触或碰撞。这种狭窄空间中的内窥操作,对于一般少冗余自由度刚性机器人,由于其运动学灵活性有限,通常不能完成。
本实施例提供的线驱动连续机器人内窥操作的协同运动规划方法,能够在操作空间和位形空间同时进行协同运动规划,避免多段弹性体连续机器人的超冗余自由度机器人系统,以及运动学逆解不封闭引起的机器人机械臂运动控制漂移问题,实现多段弹性体连续机器人机械臂安全内窥操作,安全性高;并且避免了定时定期进行关节空间位形的重新标定,提高了机器人的工作效率。
本发明还提供了线驱动连续机器人内窥操作的协同运动规划系统,图5为本发明实施例线驱动连续机器人内窥操作的协同运动规划系统的结构示意图。
参见图5,实施例的协同运动规划系统包括:
模型获取模块501,用于获取线驱动连续机器人的连续机械臂;所述连续机械臂包括m段弹性体。
划分模块502,用于将所述连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂;所述第一虚拟机械臂由所述连续机械臂中从基础端开始的连续s段的弹性体组成;所述第二虚拟机械臂由所述连续机械臂中从s+1段开始的连续m-s段的弹性体组成;m、s均为正整数,且m>s。
路径获取模块503,用于获取所述第一虚拟机械臂的操作空间预设运动路径和所述第二虚拟机械臂的关节空间预设运动路径。
第一计算模块504,用于依据所述操作空间预设运动路径,计算第一关节空间微分运动规律;所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律。
第二计算模块505,用于依据所述关节空间预设运动路径,计算第二关节空间微分运动规律;所述第二关节空间微分运动规律为所述第二虚拟机械臂在所述关节空间预设运动路径条件下的关节空间微分运动规律。
第三计算模块506,用于依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律,得到所述连续机械臂在位形空间的微分运动规律;所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。
作为一种可选的实施方式,所述第一计算模块504,具体包括:
方程建立单元,用于依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
其中,表示操作空间预设运动路径P的微分运动规律,表示第一虚拟机械臂的s段弹性体的雅克比矩阵,表示关节空间的微分运动规律;
逆解计算单元,用于对所述运动学方程求逆解,得到第一关节空间微分运动规律所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律。
作为一种可选的实施方式,所述第三计算模块506,具体为:
其中,表示连续机械臂在位形空间的微分运动规律,表示第一关节空间微分运动规律,表示第二关节空间微分运动规律。
本实施例的线驱动连续机器人内窥操作的协同运动规划系统,能够在操作空间和位形空间同时进行协同运动规划,能避免位形漂移引起的意外接触或碰撞,安全性高,且工作效率高。
对于实施例公开的系统而言,由于其与实施例公开的方法相对应,所以描述的比较简单,相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处。综上所述,本说明书内容不应理解为对本发明的限制。
Claims (8)
1.线驱动连续机器人内窥操作的协同运动规划方法,其特征在于,包括:
获取线驱动连续机器人的连续机械臂;所述连续机械臂包括m段弹性体;
将所述连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂;所述第一虚拟机械臂由所述连续机械臂中从基础端开始的连续s段的弹性体组成;所述第二虚拟机械臂由所述连续机械臂中从s+1段开始的连续m-s段的弹性体组成;m、s均为正整数,且m>s;
获取所述第一虚拟机械臂的操作空间预设运动路径和所述第二虚拟机械臂的关节空间预设运动路径;
依据所述操作空间预设运动路径,计算第一关节空间微分运动规律;所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律;
依据所述关节空间预设运动路径,计算第二关节空间微分运动规律;所述第二关节空间微分运动规律为所述第二虚拟机械臂在所述关节空间预设运动路径条件下的关节空间微分运动规律;
依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律,得到所述连续机械臂在位形空间的微分运动规律;所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。
2.根据权利要求1所述的线驱动连续机器人内窥操作的协同运动规划方法,其特征在于,所述依据所述操作空间预设运动路径,计算第一关节空间微分运动规律,具体包括:
依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
其中,表示操作空间预设运动路径P的微分运动规律,表示第一虚拟机械臂的s段弹性体的雅克比矩阵,表示关节空间的微分运动规律;
对所述运动学方程求逆解,得到第一关节空间微分运动规律所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律。
3.根据权利要求2所述的线驱动连续机器人内窥操作的协同运动规划方法,其特征在于,所述依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律,得到所述连续机械臂在位形空间的微分运动规律,具体为:
其中,表示连续机械臂在位形空间的微分运动规律,表示第一关节空间微分运动规律,表示第二关节空间微分运动规律,P表示对于第一虚拟机械臂的操作空间预设运动路径,Q表示对于第二虚拟机械臂的关节空间预设运动路径。
4.根据权利要求2所述的线驱动连续机器人内窥操作的协同运动规划方法,其特征在于,所述第一虚拟机械臂的s段弹性体的雅克比矩阵具体为:
其中,表示伴随矩阵, 表示第i-1段弹性体基础端面的坐标系相对于机器人基础坐标系的旋转矢量,表示第i-1段弹性体基础端面的坐标系的原点在机器人基础坐标系中的位置矢量;
其中,表示第i-1段弹性体自由端面的坐标系到第i-1段弹性体基础端面的坐标系之间的坐标变换矩阵,Θi-1表示第i-1段弹性体在位形空间中的运动学变量,φi-1表示第i-1段弹性体自由端面的坐标系在基础端面的坐标系中的旋转角,θi-1表示第i-1段弹性体自由端面与基础端面之间的二面角,κi-1表示第i-1段弹性体的形状曲率,li-1表示第i-1段弹性体的长度,i=1,2,...,m。
5.根据权利要求1所述的线驱动连续机器人内窥操作的协同运动规划方法,其特征在于,所述第一虚拟机械臂的空间维数与所述操作空间预设运动路径的空间维数相同。
6.线驱动连续机器人内窥操作的协同运动规划系统,其特征在于,包括:
模型获取模块,用于获取线驱动连续机器人的连续机械臂;所述连续机械臂包括m段弹性体;
划分模块,用于将所述连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂;所述第一虚拟机械臂由所述连续机械臂中从基础端开始的连续s段的弹性体组成;所述第二虚拟机械臂由所述连续机械臂中从s+1段开始的连续m-s段的弹性体组成;m、s均为正整数,且m>s;
路径获取模块,用于获取所述第一虚拟机械臂的操作空间预设运动路径和所述第二虚拟机械臂的关节空间预设运动路径;
第一计算模块,用于依据所述操作空间预设运动路径,计算第一关节空间微分运动规律;所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律;
第二计算模块,用于依据所述关节空间预设运动路径,计算第二关节空间微分运动规律;所述第二关节空间微分运动规律为所述第二虚拟机械臂在所述关节空间预设运动路径条件下的关节空间微分运动规律;
第三计算模块,用于依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律,得到所述连续机械臂在位形空间的微分运动规律;所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。
7.根据权利要求6所述的线驱动连续机器人内窥操作的协同运动规划系统,其特征在于,所述第一计算模块,具体包括:
方程建立单元,用于依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
其中,表示操作空间预设运动路径P的微分运动规律,表示第一虚拟机械臂的s段弹性体的雅克比矩阵,表示关节空间的微分运动规律;
逆解计算单元,用于对所述运动学方程求逆解,得到第一关节空间微分运动规律所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律。
8.根据权利要求7所述的线驱动连续机器人内窥操作的协同运动规划系统,其特征在于,所述第三计算模块,具体为:
其中,表示连续机械臂在位形空间的微分运动规律,表示第一关节空间微分运动规律,表示第二关节空间微分运动规律。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811587658.1A CN109434838B (zh) | 2018-12-25 | 2018-12-25 | 线驱动连续机器人内窥操作的协同运动规划方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811587658.1A CN109434838B (zh) | 2018-12-25 | 2018-12-25 | 线驱动连续机器人内窥操作的协同运动规划方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109434838A true CN109434838A (zh) | 2019-03-08 |
CN109434838B CN109434838B (zh) | 2020-07-24 |
Family
ID=65535367
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811587658.1A Active CN109434838B (zh) | 2018-12-25 | 2018-12-25 | 线驱动连续机器人内窥操作的协同运动规划方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109434838B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110744540A (zh) * | 2019-09-26 | 2020-02-04 | 北京航天测控技术有限公司 | 一种工业机器人变阻抗控制器参数整定的方法 |
CN113282077A (zh) * | 2021-04-08 | 2021-08-20 | 浙江工业大学 | 基于正态分布rrt*和雅可比矩阵的同心管机器人运动规划方法 |
CN113510706A (zh) * | 2021-07-16 | 2021-10-19 | 华中科技大学 | 一种连续体机器人的轨迹跟随运动规划方法和系统 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160016319A1 (en) * | 2010-07-08 | 2016-01-21 | Vanderbilt University | Continuum devices and control methods thereof |
US9364289B2 (en) * | 2013-10-09 | 2016-06-14 | Wisconsin Alumni Research Foundation | Interleaved manipulator |
CN107398903A (zh) * | 2017-09-04 | 2017-11-28 | 杭州电子科技大学 | 工业机械手臂执行端的轨迹控制方法 |
US20180125591A1 (en) * | 2016-11-08 | 2018-05-10 | The Board Of Trustees Of The Leland Stanford Junior University | Method for navigating a robotic surgical catheter |
-
2018
- 2018-12-25 CN CN201811587658.1A patent/CN109434838B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160016319A1 (en) * | 2010-07-08 | 2016-01-21 | Vanderbilt University | Continuum devices and control methods thereof |
US9364289B2 (en) * | 2013-10-09 | 2016-06-14 | Wisconsin Alumni Research Foundation | Interleaved manipulator |
US20180125591A1 (en) * | 2016-11-08 | 2018-05-10 | The Board Of Trustees Of The Leland Stanford Junior University | Method for navigating a robotic surgical catheter |
CN107398903A (zh) * | 2017-09-04 | 2017-11-28 | 杭州电子科技大学 | 工业机械手臂执行端的轨迹控制方法 |
Non-Patent Citations (2)
Title |
---|
SIMAAN.N等: "A dexterous system for laryngeal surgery", 《IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 * |
胡海燕等: "线驱动连续型机器人的运动学分析与仿真", 《机械工程学报》 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110744540A (zh) * | 2019-09-26 | 2020-02-04 | 北京航天测控技术有限公司 | 一种工业机器人变阻抗控制器参数整定的方法 |
CN113282077A (zh) * | 2021-04-08 | 2021-08-20 | 浙江工业大学 | 基于正态分布rrt*和雅可比矩阵的同心管机器人运动规划方法 |
CN113282077B (zh) * | 2021-04-08 | 2022-06-17 | 浙江工业大学 | 基于正态分布和雅可比矩阵的同心管机器人运动规划方法 |
CN113510706A (zh) * | 2021-07-16 | 2021-10-19 | 华中科技大学 | 一种连续体机器人的轨迹跟随运动规划方法和系统 |
CN113510706B (zh) * | 2021-07-16 | 2022-04-01 | 华中科技大学 | 一种连续体机器人的轨迹跟随运动规划方法和系统 |
Also Published As
Publication number | Publication date |
---|---|
CN109434838B (zh) | 2020-07-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109434838B (zh) | 线驱动连续机器人内窥操作的协同运动规划方法及系统 | |
CN106844951B (zh) | 基于分段几何法求解超冗余机器人逆运动学的方法及系统 | |
CN108527368B (zh) | 柔性支撑串联工业机器人作业最优初始位姿确定方法 | |
EP3978203A1 (en) | Singularity-free kinematic parameterization of soft robot manipulators | |
CN111791234A (zh) | 一种狭窄空间内多机器人空间工作位置防撞控制算法 | |
Huang et al. | A geometric algebra algorithm for the closed-form forward displacement analysis of 3-PPS parallel mechanisms | |
CN116330267A (zh) | 一种基于工业机器人腕部奇异点计算的控制方法 | |
Hu et al. | CGA-based approach for the inverse displacement of serial-parallel manipulators | |
de Oliveira et al. | A new approach to singularity-free inverse kinematics using dual-quaternionic error chains in the davies method | |
Sanjuan et al. | Closed form solution for direct and inverse kinematics of a US-RS-RPS 2-DOF parallel robot | |
He et al. | Design and kinematic analysis of a novel hybrid kinematic mechanism with seven-degrees-of-freedom and variable topology for operation in space | |
Gu et al. | Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following | |
Cui et al. | A novel inverse kinematics solution for a 7-DOF humanoid manipulator | |
Sánchez-Alonso et al. | Kinematic analysis of a novel 2 (3-RUS) parallel manipulator | |
Ghosal | Kinematics of serial manipulators | |
Lopes | Complete dynamic modelling of a moving base 6-dof parallel manipulator | |
Li et al. | Dynamic modeling and analysis for 6-DOF industrial robots | |
Qu et al. | A new method for isotropic analysis of limited DOF parallel manipulators with terminal constraints | |
Jingwei et al. | Welding Robot Kinematics Analysis and Trajectory Planning. | |
Sun et al. | Design and modeling of a cable-driven hollow continuum manipulator | |
Faulkner et al. | A generalised, modular, approach for the forward kinematics of continuum soft robots with sections of constant curvature | |
Wang et al. | A novel 2-SUR 6-DOF parallel manipulator actuated by spherical motion generators | |
Ge et al. | Kinematics modeling and analysis of manipulator using the dual quaternion | |
Lu et al. | An optimization algorithm for trajectory planning of a 7-DOF redundant manipulator | |
Aboelnasr et al. | Novel use of the Monte-Carlo methods to visualize singularity configurations in serial manipulators |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |