CN109434838B - 线驱动连续机器人内窥操作的协同运动规划方法及系统 - Google Patents

线驱动连续机器人内窥操作的协同运动规划方法及系统 Download PDF

Info

Publication number
CN109434838B
CN109434838B CN201811587658.1A CN201811587658A CN109434838B CN 109434838 B CN109434838 B CN 109434838B CN 201811587658 A CN201811587658 A CN 201811587658A CN 109434838 B CN109434838 B CN 109434838B
Authority
CN
China
Prior art keywords
mechanical arm
joint space
space
differential motion
continuous
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.)
Active
Application number
CN201811587658.1A
Other languages
English (en)
Other versions
CN109434838A (zh
Inventor
苏婷婷
何广平
贾涛鸣
赵磊
狄杰建
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
North China University of Technology
Original Assignee
North China University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by North China University of Technology filed Critical North China University of Technology
Priority to CN201811587658.1A priority Critical patent/CN109434838B/zh
Publication of CN109434838A publication Critical patent/CN109434838A/zh
Application granted granted Critical
Publication of CN109434838B publication Critical patent/CN109434838B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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；
获取所述第一虚拟机械臂的操作空间预设运动路径和所述第二虚拟机械臂的关节空间预设运动路径；
依据所述操作空间预设运动路径，计算第一关节空间微分运动规律；所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律；
依据所述关节空间预设运动路径，计算第二关节空间微分运动规律；所述第二关节空间微分运动规律为所述第二虚拟机械臂在所述关节空间预设运动路径条件下的关节空间微分运动规律；
依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律，得到所述连续机械臂在位形空间的微分运动规律；所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。
可选的，所述依据所述操作空间预设运动路径，计算第一关节空间微分运动规律，具体包括：
依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
Figure BDA0001919508140000021
其中，
Figure BDA0001919508140000022
表示操作空间预设运动路径P的微分运动规律，
Figure BDA0001919508140000023
表示第一虚拟机械臂的s段弹性体的雅克比矩阵，
Figure BDA0001919508140000024
表示关节空间的微分运动规律；
对所述运动学方程求逆解，得到第一关节空间微分运动规律
Figure BDA0001919508140000025
所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律。
可选的，所述依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律，得到所述连续机械臂在位形空间的微分运动规律，具体为：
Figure BDA0001919508140000026
其中，
Figure BDA0001919508140000031
表示连续机械臂在位形空间的微分运动规律，
Figure BDA0001919508140000032
表示第一关节空间微分运动规律，
Figure BDA0001919508140000033
表示第二关节空间微分运动规律，P表示对于第一虚拟机械臂的操作空间预设运动路径，Q表示对于第二虚拟机械臂的关节空间预设运动路径。
可选的，所述第一虚拟机械臂的s段弹性体的雅克比矩阵具体为：
Figure BDA0001919508140000034
其中，
Figure BDA0001919508140000035
表示伴随矩阵，
Figure BDA0001919508140000036
表示第i-1段弹性体基础端面的坐标系相对于机器人基础坐标系的旋转矢量，
Figure BDA0001919508140000037
表示第i-1段弹性体基础端面的坐标系的原点在机器人基础坐标系中的位置矢量；
Figure BDA0001919508140000038
其中，
Figure BDA0001919508140000039
表示第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；
路径获取模块，用于获取所述第一虚拟机械臂的操作空间预设运动路径和所述第二虚拟机械臂的关节空间预设运动路径；
第一计算模块，用于依据所述操作空间预设运动路径，计算第一关节空间微分运动规律；所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律；
第二计算模块，用于依据所述关节空间预设运动路径，计算第二关节空间微分运动规律；所述第二关节空间微分运动规律为所述第二虚拟机械臂在所述关节空间预设运动路径条件下的关节空间微分运动规律；
第三计算模块，用于依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律，得到所述连续机械臂在位形空间的微分运动规律；所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。
可选的，所述第一计算模块，具体包括：
方程建立单元，用于依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
Figure BDA0001919508140000041
其中，
Figure BDA0001919508140000042
表示操作空间预设运动路径P的微分运动规律，
Figure BDA0001919508140000043
表示第一虚拟机械臂的s段弹性体的雅克比矩阵，
Figure BDA0001919508140000044
表示关节空间的微分运动规律；
逆解计算单元，用于对所述运动学方程求逆解，得到第一关节空间微分运动规律
Figure BDA0001919508140000045
所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律。
可选的，所述第三计算模块，具体为：
Figure BDA0001919508140000051
其中，
Figure BDA0001919508140000052
表示连续机械臂在位形空间的微分运动规律，
Figure BDA0001919508140000053
表示第一关节空间微分运动规律，
Figure BDA0001919508140000054
表示第二关节空间微分运动规律。
与现有技术相比，本发明的有益效果是：
本发明提出了线驱动连续机器人内窥操作的协同运动规划方法及系统。所述协同运动规划方法中将连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂；获取操作空间预设运动路径和关节空间预设运动路径；计算第一虚拟机械臂在操作空间预设运动路径条件下的关节空间微分运动规律以及第二虚拟机械臂在关节空间预设运动路径条件下的关节空间微分运动规律；计算连续机械臂在位形空间的微分运动规律，连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。采用本发明的方法或系统，能够在操作空间和位形空间同时进行协同运动规划，避免多段弹性体连续机器人的超冗余自由度机器人系统，以及运动学逆解不封闭引起的机器人机械臂运动控制漂移问题，实现多段弹性体连续机器人机械臂安全内窥操作，安全性高；并且避免了定时定期进行关节空间位形的重新标定，提高了机器人的工作效率。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图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)依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
Figure BDA0001919508140000071
其中，
Figure BDA0001919508140000072
表示操作空间预设运动路径P的微分运动规律，
Figure BDA0001919508140000073
表示第一虚拟机械臂的s段弹性体的雅克比矩阵，
Figure BDA0001919508140000074
表示关节空间的微分运动规律。
所述第一虚拟机械臂的s段弹性体的雅克比矩阵
Figure BDA0001919508140000075
具体为：
Figure BDA0001919508140000076
其中，
Figure BDA0001919508140000077
表示伴随矩阵，
Figure BDA0001919508140000078
表示第i-1段弹性体基础端面的坐标系相对于机器人基础坐标系的旋转矢量，
Figure BDA0001919508140000079
表示第i-1段弹性体基础端面的坐标系的原点在机器人基础坐标系中的位置矢量；
Figure BDA00019195081400000710
其中，
Figure BDA0001919508140000081
表示第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)对所述运动学方程求逆解，得到第一关节空间微分运动规律
Figure BDA0001919508140000082
所述第一关节空间微分运动规律为所述第一虚拟机械臂Aos在所述操作空间预设运动路径P条件下的关节空间微分运动规律。具体求解过程为：
Figure BDA0001919508140000083
上述步骤(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和弹性体的面内弯曲运动组成。
采用螺旋坐标
Figure BDA0001919508140000084
Figure BDA0001919508140000085
描述弹性体的运动，其中
Figure BDA0001919508140000086
和
Figure BDA0001919508140000087
分别表示弹性体的平移运动向量和旋转运动向量，ξrot表示绕z轴的旋转量，ξinp表示绕y轴的弯曲。借助于楔算子“∧”，ξrot和ξinp所示的螺旋坐标可表示为两个旋量
Figure BDA0001919508140000091
Figure BDA0001919508140000092
其中，
Figure BDA0001919508140000093
为
Figure BDA0001919508140000094
Figure BDA0001919508140000095
为
Figure BDA0001919508140000096
进一步应用指数积公式，从坐标系∑i到坐标系∑i-1的坐标变换矩阵可表示为
Figure BDA0001919508140000097
其中，
Figure BDA0001919508140000098
表示坐标系∑i相对于坐标系∑i-1的旋转矩阵，
Figure BDA00019195081400000912
表示坐标系∑i的原点在坐标系∑i-1中的位置矢量，
Figure BDA0001919508140000099
Figure BDA00019195081400000910
对于由m段弹性体组成的连续机器人，其末端操作器中心坐标系到机器人基础坐标系之间的坐标变换矩阵可表示为
Figure BDA00019195081400000911
再次应用指数积公式，从上式的时间导数可得到机械臂末端操作器的速度
Figure BDA0001919508140000101
其中
Figure BDA0001919508140000102
表示速度矢量
Figure BDA0001919508140000103
的旋量，上式的矩阵形式可表示为
Figure BDA0001919508140000104
其中
Figure BDA0001919508140000105
为连续机器人机械臂的运动学雅克比矩阵，且可以明确给出为
Figure BDA0001919508140000106
其中“∨”表示楔算子“∧”的逆算子。
利用伴随变换
Figure BDA0001919508140000107
雅克比矩阵
Figure BDA0001919508140000108
可进一步表示为更明确形式
Figure BDA0001919508140000109
其中，
Figure BDA00019195081400001010
由上述推导，得到了最终的连续机器人运动学雅克比矩阵，该运动学雅克比矩阵基于每段弹性体弯曲曲率均匀的假设。在弹性体重量较轻，机械臂受到外力较小的条件下，上述最终的连续机器人运动学雅克比矩阵的准确性能够满足实际应用。
步骤S5：依据所述关节空间预设运动路径Q，计算第二关节空间微分运动规律。所述第二关节空间微分运动规律为所述第二虚拟机械臂Asm在所述关节空间预设运动路径Q条件下的关节空间微分运动规律。
具体的，所述第二关节空间微分运动规律可通过对所述关节空间预设运动路径Q求导得到。
步骤S6：依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律，得到所述连续机械臂在位形空间的微分运动规律。
所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动。
所述步骤S6具体为：
Figure BDA0001919508140000111
其中，
Figure BDA0001919508140000112
表示连续机械臂在位形空间的微分运动规律，
Figure BDA0001919508140000113
表示第一关节空间微分运动规律，
Figure BDA0001919508140000114
表示第二关节空间微分运动规律，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在位形空间中的微分运动规律，即第二关节空间微分运动规律
Figure BDA0001919508140000115
下面本实施例提供的线驱动连续机器人内窥操作的协同运动规划方法的有效性进行了验证。
对于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在内窥操作中的目标位形曲率变量可写出为
Figure BDA0001919508140000121
给定内窥操作时基础段虚拟机械臂的末端移动速度为
Figure BDA0001919508140000122
采用上述实施例给出的协同运动规划方法，可计算获得六段弹性体连续机器人的内窥操作运动规划仿真结果图，如图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，具体包括：
方程建立单元，用于依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
Figure BDA0001919508140000131
其中，
Figure BDA0001919508140000132
表示操作空间预设运动路径P的微分运动规律，
Figure BDA0001919508140000133
表示第一虚拟机械臂的s段弹性体的雅克比矩阵，
Figure BDA0001919508140000134
表示关节空间的微分运动规律；
逆解计算单元，用于对所述运动学方程求逆解，得到第一关节空间微分运动规律
Figure BDA0001919508140000135
所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律。
作为一种可选的实施方式，所述第三计算模块506，具体为：
Figure BDA0001919508140000136
其中，
Figure BDA0001919508140000141
表示连续机械臂在位形空间的微分运动规律，
Figure BDA0001919508140000142
表示第一关节空间微分运动规律，
Figure BDA0001919508140000143
表示第二关节空间微分运动规律。
本实施例的线驱动连续机器人内窥操作的协同运动规划系统，能够在操作空间和位形空间同时进行协同运动规划，能避免位形漂移引起的意外接触或碰撞，安全性高，且工作效率高。
对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (5)

1.线驱动连续机器人内窥操作的协同运动规划方法，其特征在于，包括：
获取线驱动连续机器人的连续机械臂；所述连续机械臂包括m段弹性体；
将所述连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂；所述第一虚拟机械臂由所述连续机械臂中从基础端开始的连续s段的弹性体组成；所述第二虚拟机械臂由所述连续机械臂中从s+1段开始的连续m-s段的弹性体组成；m、s均为正整数，且m＞s；
获取所述第一虚拟机械臂的操作空间预设运动路径和所述第二虚拟机械臂的关节空间预设运动路径；
依据所述操作空间预设运动路径，计算第一关节空间微分运动规律；所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律；
依据所述关节空间预设运动路径，计算第二关节空间微分运动规律；所述第二关节空间微分运动规律为所述第二虚拟机械臂在所述关节空间预设运动路径条件下的关节空间微分运动规律；
依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律，得到所述连续机械臂在位形空间的微分运动规律；所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动；
所述依据所述操作空间预设运动路径，计算第一关节空间微分运动规律，具体包括：
依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
Figure RE-FDA0002472598740000011
其中，
Figure RE-FDA0002472598740000012
表示操作空间预设运动路径P的微分运动规律，
Figure RE-FDA0002472598740000013
表示第一虚拟机械臂的s段弹性体的雅克比矩阵，
Figure RE-FDA0002472598740000014
表示关节空间的微分运动规律；
对所述运动学方程求逆解，得到第一关节空间微分运动规律
Figure RE-FDA0002472598740000015
所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律；
所述第一虚拟机械臂的s段弹性体的雅克比矩阵具体为：
Figure RE-FDA0002472598740000021
其中，
Figure RE-FDA0002472598740000022
表示伴随矩阵，
Figure RE-FDA0002472598740000023
Figure RE-FDA0002472598740000024
表示第i-1段弹性体基础端面的坐标系相对于机器人基础坐标系的旋转矢量，
Figure RE-FDA0002472598740000025
表示第i-1段弹性体基础端面的坐标系的原点在机器人基础坐标系中的位置矢量；
Figure RE-FDA0002472598740000026
其中，Ti 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.根据权利要求1所述的线驱动连续机器人内窥操作的协同运动规划方法，其特征在于，所述依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律，得到所述连续机械臂在位形空间的微分运动规律，具体为：
Figure RE-FDA0002472598740000027
其中，
Figure RE-FDA0002472598740000031
表示连续机械臂在位形空间的微分运动规律，
Figure RE-FDA0002472598740000032
表示第一关节空间微分运动规律，
Figure RE-FDA0002472598740000033
表示第二关节空间微分运动规律，P表示对于第一虚拟机械臂的操作空间预设运动路径，Q表示对于第二虚拟机械臂的关节空间预设运动路径。
3.根据权利要求1所述的线驱动连续机器人内窥操作的协同运动规划方法，其特征在于，所述第一虚拟机械臂的空间维数与所述操作空间预设运动路径的空间维数相同。
4.线驱动连续机器人内窥操作的协同运动规划系统，其特征在于，包括：
模型获取模块，用于获取线驱动连续机器人的连续机械臂；所述连续机械臂包括m段弹性体；
划分模块，用于将所述连续机械臂划分为第一虚拟机械臂和第二虚拟机械臂；所述第一虚拟机械臂由所述连续机械臂中从基础端开始的连续s段的弹性体组成；所述第二虚拟机械臂由所述连续机械臂中从s+1段开始的连续m-s段的弹性体组成；m、s均为正整数，且m＞s；
路径获取模块，用于获取所述第一虚拟机械臂的操作空间预设运动路径和所述第二虚拟机械臂的关节空间预设运动路径；
第一计算模块，用于依据所述操作空间预设运动路径，计算第一关节空间微分运动规律；所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律；
第二计算模块，用于依据所述关节空间预设运动路径，计算第二关节空间微分运动规律；所述第二关节空间微分运动规律为所述第二虚拟机械臂在所述关节空间预设运动路径条件下的关节空间微分运动规律；
第三计算模块，用于依据所述第一关节空间微分运动规律和所述第二关节空间微分运动规律，得到所述连续机械臂在位形空间的微分运动规律；所述连续机械臂在位形空间的微分运动规律用于实现线驱动连续机器人在内窥操作的协同运动；
所述第一计算模块，具体包括：
方程建立单元，用于依据所述操作空间预设运动路径建立所述第一虚拟机械臂的运动学方程
Figure RE-FDA0002472598740000041
其中，
Figure RE-FDA0002472598740000042
表示操作空间预设运动路径P的微分运动规律，
Figure RE-FDA0002472598740000043
表示第一虚拟机械臂的s段弹性体的雅克比矩阵，
Figure RE-FDA0002472598740000044
表示关节空间的微分运动规律；
逆解计算单元，用于对所述运动学方程求逆解，得到第一关节空间微分运动规律
Figure RE-FDA0002472598740000045
所述第一关节空间微分运动规律为所述第一虚拟机械臂在所述操作空间预设运动路径条件下的关节空间微分运动规律；
所述第一虚拟机械臂的s段弹性体的雅克比矩阵具体为：
Figure RE-FDA0002472598740000046
其中，
Figure RE-FDA0002472598740000047
表示伴随矩阵，
Figure RE-FDA0002472598740000048
Figure RE-FDA0002472598740000049
表示第i-1段弹性体基础端面的坐标系相对于机器人基础坐标系的旋转矢量，
Figure RE-FDA00024725987400000410
表示第i-1段弹性体基础端面的坐标系的原点在机器人基础坐标系中的位置矢量；
Figure RE-FDA00024725987400000411
其中，Ti 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.根据权利要求4所述的线驱动连续机器人内窥操作的协同运动规划系统，其特征在于，所述第三计算模块，具体为：
Figure RE-FDA0002472598740000051
其中，
Figure RE-FDA0002472598740000052
表示连续机械臂在位形空间的微分运动规律，
Figure RE-FDA0002472598740000053
表示第一关节空间微分运动规律，
Figure RE-FDA0002472598740000054
表示第二关节空间微分运动规律。
CN201811587658.1A 2018-12-25 2018-12-25 线驱动连续机器人内窥操作的协同运动规划方法及系统 Active CN109434838B (zh)

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 CN109434838A (zh) 2019-03-08
CN109434838B true 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)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110744540B (zh) * 2019-09-26 2021-03-23 北京航天测控技术有限公司 一种工业机器人变阻抗控制器参数整定的方法
CN113282077B (zh) * 2021-04-08 2022-06-17 浙江工业大学 基于正态分布和雅可比矩阵的同心管机器人运动规划方法
CN113510706B (zh) * 2021-07-16 2022-04-01 华中科技大学 一种连续体机器人的轨迹跟随运动规划方法和系统

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10737398B2 (en) * 2010-07-08 2020-08-11 Vanderbilt University Continuum devices and control methods thereof
US9364289B2 (en) * 2013-10-09 2016-06-14 Wisconsin Alumni Research Foundation Interleaved manipulator
US10603124B2 (en) * 2016-11-08 2020-03-31 The Board Of Trustees Of The Leland Stanford Junior University Method for navigating a robotic surgical catheter
CN107398903B (zh) * 2017-09-04 2020-06-30 杭州电子科技大学 工业机械手臂执行端的轨迹控制方法

Also Published As

Publication number Publication date
CN109434838A (zh) 2019-03-08

Similar Documents

Publication Publication Date Title
CN111300425B (zh) 一种超冗余机械臂末端轨迹运动规划方法
CN109434838B (zh) 线驱动连续机器人内窥操作的协同运动规划方法及系统
Li et al. Kinematics analysis of a novel over-constrained three degree-of-freedom spatial parallel manipulator
CN106844951B (zh) 基于分段几何法求解超冗余机器人逆运动学的方法及系统
Xu et al. Design and tension modeling of a novel cable-driven rigid snake-like manipulator
Huang et al. A geometric algebra algorithm for the closed-form forward displacement analysis of 3-PPS parallel mechanisms
Rao et al. Position analysis of spatial 3-RPS parallel manipulator
CN111791234A (zh) 一种狭窄空间内多机器人空间工作位置防撞控制算法
EP3978203A1 (en) Singularity-free kinematic parameterization of soft robot manipulators
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
Zhao Singularity, isotropy, and velocity transmission evaluation of a three translational degrees-of-freedom 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
Sánchez-Alonso et al. Kinematic analysis of a novel 2 (3-RUS) parallel manipulator
Ghosal Kinematics of serial manipulators
Cui et al. A novel inverse kinematics solution for a 7-DOF humanoid manipulator
Lopes Complete dynamic modelling of a moving base 6-dof parallel manipulator
Wu et al. Parallel PnP Robots
Qu et al. A new method for isotropic analysis of limited DOF parallel manipulators with terminal constraints
Gu et al. Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following
Faulkner et al. A generalised, modular, approach for the forward kinematics of continuum soft robots with sections of constant curvature
Yue Kinematic modelling and simulation of LeArm robot
Sun et al. Design and modeling of a cable-driven hollow continuum manipulator
Ge et al. Kinematics modeling and analysis of manipulator using the dual quaternion

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