CN114012728B - 适用于两段6自由度连续体机械臂的逆运动学求解方法 - Google Patents

适用于两段6自由度连续体机械臂的逆运动学求解方法 Download PDF

Info

Publication number
CN114012728B
CN114012728B CN202111338689.5A CN202111338689A CN114012728B CN 114012728 B CN114012728 B CN 114012728B CN 202111338689 A CN202111338689 A CN 202111338689A CN 114012728 B CN114012728 B CN 114012728B
Authority
CN
China
Prior art keywords
section
arc curve
formula
arm
mechanical arm
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
CN202111338689.5A
Other languages
English (en)
Other versions
CN114012728A (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.)
Harbin Institute of Technology Weihai
Original Assignee
Harbin Institute of Technology Weihai
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 Harbin Institute of Technology Weihai filed Critical Harbin Institute of Technology Weihai
Priority to CN202111338689.5A priority Critical patent/CN114012728B/zh
Publication of CN114012728A publication Critical patent/CN114012728A/zh
Application granted granted Critical
Publication of CN114012728B publication Critical patent/CN114012728B/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/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明涉及一种适用于两段6自由度连续体机械臂的逆运动学求解方法，其为了解决如何针对全自由度连续体机械臂实现精确运动控制的技术问题。本发明采用虚拟连杆法，将两段串联组成的连续体机械臂等效为一种连杆型机械臂，并给出各运动副间的约束；将机械臂逆运动学模型的物理描述转换为数学描述；将模型的数学描述转换为函数表达式，并进行求解。

Description

适用于两段6自由度连续体机械臂的逆运动学求解方法
技术领域
本发明涉及软体机械臂技术领域，具体而言，涉及一种适用于两段6自由度连续体机械臂的逆运动学求解方法。
背景技术
连续体机械臂(或称软体机械臂)是一种能呈现诸如象鼻、章鱼触手等动物运动的机器人。不同于刚性机器人，此类机器人的运动由本体的变形产生，而不是传统的机械运动副。它们往往具有很好的运动灵活性、本体呈细长结构，具有被动柔顺变形能力，在单孔腔镜手术、航空发动机内部检查、核电站维修、水下捕捞以及腔内吸取、喷涂等领域具有独特的优势。参考申请公布号为CN109895079A，专利名称为“一种全自由度连续体机械臂”的发明专利申请，其采用镍钛合金螺旋弹簧作为机械臂本体，四根牵引绳贯穿整个镍钛合金螺旋弹簧，用四根牵引绳驱动镍钛合金螺旋弹簧实现全自由度弯曲，柔顺性、灵活性好。
针对申请公布号为CN109895079A的发明专利申请公开的全自由度连续体机械臂，研究精确的运动控制方法是本领域技术人员亟待解决的技术问题。
发明内容
本发明就是为了解决如何针对全自由度连续体机械臂实现精确运动控制的技术问题，提供了一种适用于两段6自由度连续体机械臂的逆运动学求解方法。
本发明的思想是：
第一步：采用常曲率假设，建立每一段机械臂的正运动学和逆运动学模型，给出驱动空间、关节空间以及操作空间变量之间的关系；
第二步：采用虚拟连杆法，将两段串联组成的连续体机械臂等效为一种连杆型机械臂，并给出各运动副间的约束；
第三步：将机械臂逆运动学模型的物理描述转换为数学描述；
第四步：将模型的数学描述转换为函数表达式，并进行求解；
第五步：讨论机械臂逆解的存在情况，给出一般情况下机械臂逆解的解析模型并建立数值求解算法。
本发明提供一种适用于两段6自由度连续体机械臂的逆运动学求解方法，包括以下步骤：
步骤1，定义软体臂是由第一段圆弧曲线和第二段圆弧曲线串联；
步骤2，将软体臂等效成由连杆OJ1、J1J2、J2E组成的串联型机械臂；
步骤3，已知空间中一点E的坐标(xE，yE，zE)及经过该点E的直线EJ2的方向向量(a，b，c)，求该直线J2E上的一点J2，且满足以下约束关系：
Figure BDA0003351544500000021
通过公式(2)计算第一段圆弧曲线的其弯曲角度θ1
Figure BDA0003351544500000022
公式(2)中，
Figure BDA0003351544500000023
为世界坐标系Z轴的单位向量；
通过公式(3)计算第一段圆弧曲线的曲率半径，
Figure BDA0003351544500000024
通过公式(4)计算第一段圆弧曲线绕Z轴扭转角，
Figure BDA0003351544500000025
通过公式(5)计算第二段圆弧曲线的其弯曲角度θ2
Figure BDA0003351544500000026
通过公式(6)计算第二段圆弧曲线的曲率半径，
Figure BDA0003351544500000027
第二段圆弧曲线相对于第一段圆弧曲线的扭转角度为
Figure BDA0003351544500000028
其相当于第一段圆弧曲线所在平面J1OA的法向量n1和第二段圆弧曲线所在平面J2EA法向量n2之间的夹角，由下式(7)计算出扭转角度为
Figure BDA0003351544500000031
Figure BDA0003351544500000032
公式(7)中，
Figure BDA0003351544500000033
点A的坐标(xA，yA，zA)可以通过下式(8)求得，
Figure BDA0003351544500000034
第二段圆弧曲线的扭转角为：
Figure BDA0003351544500000035
步骤4，计算软体臂的关节空间向量：
通过公式(12)计算kc
Figure BDA0003351544500000036
Figure BDA0003351544500000037
为软体臂按要求运动空间变量，其中，xE为软体臂末端点的目标位置向量，
Figure BDA0003351544500000038
为软体臂末端的目标方向向量；kl为软体臂的两段长度之比，采用二分法计算在给定末端位姿xd和给定kl＝kd条件下的软体臂逆解，通过表1的算法计算关节空间向量；表一中，zU、zL分别为区间上限、下限；函数
Figure BDA0003351544500000039
Err为软体臂期望的kd与迭代过程中的两段长度之比kl的数值算法给定的误差限；g(z1)为已知z1求软体臂关节空间变量的函数，可由公式(1)-式(9)表示。
表1：
Figure BDA0003351544500000041
优选地，Err＝10-6。
本发明的有益效果是：逆运动学求解过程稳定、快速、准确。
本发明进一步的特征，将在以下具体实施方式的描述中，得以清楚地记载。
附图说明
图1是6自由度连续体机械臂的结构示意图；
图2是伪刚体建模示意图，其中图(a)是机械臂等效曲线模型，图(b)是等效的伪刚体模型；
图3是伪刚体模型变量与机械臂关节空间变量间的几何关系；
图4是逆运动学求解方法的流程图；
图中符号说明：
1.螺旋弹簧，2.固定板，3.第一电机，4.第一绕线辊，5.第一钢丝绳，6.第二钢丝绳，7.第三钢丝绳，8.第四钢丝绳，9.第五钢丝绳，10.第六钢丝绳。
具体实施方式
以下参照附图，以具体实施例对本发明作进一步详细说明。
如图1所示，6自由度连续体机械臂的螺旋弹簧1固定在固定板2上，螺旋弹簧1被六根钢丝绳驱动，第一钢丝绳5、第二钢丝绳6、第三钢丝绳7、第四钢丝绳8、第五钢丝绳9、第六钢丝绳10穿过螺旋弹簧1，第一绕线辊4与第一电机3的输出轴连接，第一钢丝绳5缠绕在第一绕线辊4上。同理，第二绕线辊与第二电机的输出轴连接，第二钢丝绳6缠绕在第二绕线辊上，第三绕线辊与第三电机的输出轴连接，第三钢丝绳7缠绕在第三绕线辊上，第四绕线辊与第四电机的输出轴连接，第四钢丝绳8缠绕在第四绕线辊上，第五绕线辊与第五电机的输出轴连接，第五钢丝绳9缠绕在第五绕线辊上，第六绕线辊与第六电机的输出轴连接，第六钢丝绳10缠绕在第六绕线辊上。螺旋弹簧1作为软体臂，研究控制该软体臂伸缩、弯曲运动的方法时，可以认为螺旋弹簧1由S-1、S-2两段串联组成，每一段均可实现伸缩、弯曲运动。
参考图2中的图(a)，由两段串联而成的软体臂的空间位姿相当于两段圆弧曲线通过A点相连，且第二段圆弧曲线与第一段圆弧曲线相切于A点。在O点处有连结大地并与第一段圆弧曲线相切的旋转运动副，在A点存在一个与圆弧曲线的切线重合的旋转运动副连结第一段和第二段软体臂曲线。因此，为方便运动控制过程中位置逆运动学求解，将软体臂虚拟等效为串联连杆机械臂。
参考图2，将软体臂的两段圆弧状中轴线的切线设为伪刚体，其中O点为旋转副，对于第一段圆弧曲线，J1处为球铰关节，OJ1和J1A为两个长度相同的连杆，其上具有可伸缩的滑动副，且始终保持OJ1＝J1A；第二段圆弧曲线与第一段圆弧曲线类似，其中J2处为球铰关节，且J2A＝J2E。由于O点处几何约束使得连杆OJ1始终与世界坐标系Z轴重合。由此，由两段构成的6自由度软体机械臂等效成由连杆OJ1、J1J2、J2E组成的串联型机械臂。
逆运动学求解过程是，已知末端位置向量E(xE，yE，zE)和在该点的切线方向的切向量
Figure BDA0003351544500000051
求解A点和J2点的坐标，继而求出软体臂的关节空间向量。因此，已知空间中一点E的坐标(xE，yE，zE)及经过该点E的直线EJ2的方向向量(a，b，c)，求该直线J2E上的一点J2，且满足以下约束关系：
Figure BDA0003351544500000061
参考图3，第一段圆弧曲线上，可以看出其弯曲角度θ1等于世界坐标系Z轴的单位向量与
Figure BDA0003351544500000062
之间的夹角，由余弦定理可知：
Figure BDA0003351544500000063
公式(2)中，
Figure BDA0003351544500000064
为世界坐标系Z轴的单位向量。
第一段圆弧曲线的曲率半径为：
Figure BDA0003351544500000065
第一段圆弧曲线绕Z轴扭转角为：
Figure BDA0003351544500000066
同理，可以求得第二段圆弧曲线的3个关节变量，第二段圆弧曲线的弯曲角度θ2为：
Figure BDA0003351544500000067
第二段圆弧曲线的曲率半径为：
Figure BDA0003351544500000068
第二段圆弧曲线相对于第一段圆弧曲线的扭转角度为
Figure BDA0003351544500000069
其相当于第一段圆弧曲线所在平面J1OA的法向量n1和第二段圆弧曲线所在平面J2EA法向量n2之间的夹角，因此由下式(7)计算出扭转角度为
Figure BDA00033515445000000610
Figure BDA00033515445000000611
公式(7)中，
Figure BDA00033515445000000612
点A的坐标(xA，yA，zA)可以通过下式(8)求得。
Figure BDA00033515445000000613
第二段圆弧曲线的扭转角为：
Figure BDA0003351544500000071
下一步计算软体臂的关节空间向量，参考图4的流程，首先给出直线J2E的方向式方程：
Figure BDA0003351544500000072
因为向量
Figure BDA0003351544500000073
和其方向向量
Figure BDA0003351544500000074
同向，所以有比例常数kc＞0恒成立。
对公式(10)展开计算得出下式：
Figure BDA0003351544500000075
从公式(11)计算出kc：
Figure BDA0003351544500000076
定义kc：
Figure BDA0003351544500000077
公式(13)中，
Figure BDA0003351544500000078
求公式(13)的导数：
Figure BDA0003351544500000079
可以看出，导函数值的正负取决于分子BsolCsol-AsolDsol，且有：
Figure BDA00033515445000000710
若BsolCsol-AsolDsol＞0，函数Fsol单调递增，此时z1的最小值为
Figure BDA0003351544500000081
且Fsol(0)的±取决于zE。当zE＞0时，有kc＞0恒成立，此时z1的取值范围为(0，+∞)；当zE＜0时，z1的取值范围为(-Asol/Bsol，+∞)。
若BsolCsol-AsolDsol＜0，函数Fsol单调递减，注意此函数具有无穷间断点z1＝-Csol/Dsol，但由于BsolCsol-AsolDsol＜0，可知-Csol/Dsol＜-Asol/Bsol。则满足kc＞0的z1取值范围为(max(0；-Csol/Dsol)，-Asol/Bsol)。若-Asol/Bsol＜0，即zE＜0，则kc不存在，及此时机械臂的逆解不存在。
通常情况下，软体臂工作时其末端点坐标始终位于zE＞0的象限内，由公式(12)可以以直接给出kc的初值。这使图4的流程执行大大简化。可以证明，在zE＞0时BsolCsol-AsolDsol≤0恒成立。
Figure BDA0003351544500000082
为软体臂按要求运动空间变量，其中，xE为软体臂末端点的目标位置向量，
Figure BDA0003351544500000083
为软体臂末端的目标方向向量。软体臂的两段长度之比kl与函数kc值之间存在单调递减函数关系，因此可以采用二分法可稳定快速计算在给定末端位姿xd和给定kl＝kd条件下的软体臂逆解。对于大多数情况，满足图4中的算法分支BsolCsol-AsolDsol＜0且zE＞0，二分法可快速给出图4中的逆运动学计算，如表1所示，表1的算法中，zU、zL分别为区间上限、下限；函数
Figure BDA0003351544500000084
Err为软体臂期望的kd与迭代过程中的两段长度之比kl的数值算法给定的误差限。g(z1)为已知z1求软体臂关节空间变量的函数，可由式(1)-式(9)表示。
表1：
Figure BDA0003351544500000085
计算出软体臂的关节空间变量后，就可以根据关节空间变量计算六个驱动电机的转动角度。
以上示意性的对本发明及其实施方式进行了描述，该描述没有限制性，附图中所示的也只是本发明的实施方式之一，实际的结构并不局限于此。所以，如果本领域的技术人员受其启示，在不脱离本发明创造宗旨的情况下，采用其它形式的零件构型、驱动装置以及连接方式不经创造性的设计与该技术方案相似的结构方式及实施例，均应属于本发明的保护范围。

Claims (2)

1.一种适用于两段6自由度连续体机械臂的逆运动学求解方法，其特征在于，包括以下步骤：
步骤1，定义软体臂是由第一段圆弧曲线和第二段圆弧曲线串联；
步骤2，将软体臂等效成由连杆OJ1、J1J2、J2E组成的串联型机械臂；
步骤3，已知空间中一点E的坐标(xE，yE，zE)及经过该点E的直线EJ2的方向向量(a,b,c)，求该直线J2E上的一点J2，且满足以下约束关系：
Figure FDA0004135590860000011
通过公式(2)计算第一段圆弧曲线的其弯曲角度θ1
Figure FDA0004135590860000012
公式(2)中，
Figure FDA0004135590860000013
为世界坐标系Z轴的单位向量；
通过公式(3)计算第一段圆弧曲线的曲率半径，
Figure FDA0004135590860000014
通过公式(4)计算第一段圆弧曲线绕Z轴扭转角，
Figure FDA0004135590860000015
通过公式(5)计算第二段圆弧曲线的其弯曲角度θ2
Figure FDA0004135590860000016
通过公式(6)计算第二段圆弧曲线的曲率半径，
Figure FDA0004135590860000017
第二段圆弧曲线相对于第一段圆弧曲线的扭转角度为
Figure FDA0004135590860000018
其相当于第一段圆弧曲线所在平面J1OA的法向量n1和第二段圆弧曲线所在平面J2EA法向量n2之间的夹角，由下式(7)计算出扭转角度为
Figure FDA0004135590860000019
Figure FDA00041355908600000110
公式(7)中，
Figure FDA00041355908600000111
点A的坐标(xA，yA，zA)通过下式(8)求得，
Figure FDA0004135590860000021
第二段圆弧曲线的扭转角为：
Figure FDA0004135590860000022
步骤4，计算软体臂的关节空间向量：
通过公式(12)计算kc
Figure FDA0004135590860000023
Figure FDA0004135590860000024
为软体臂按要求运动空间变量，其中，xE为软体臂末端点的目标位置向量，
Figure FDA0004135590860000025
为软体臂末端的目标方向向量；kl为软体臂的两段长度之比，采用二分法计算在给定末端位姿xd和给定kl＝kd条件下的软体臂逆解，通过如下算法计算关节空间向量：zU、zL分别为区间上限、下限；函数
Figure FDA0004135590860000026
Err为软体臂期望的kd与迭代过程中的两段长度之比kl的数值算法给定的误差限；g(z1)为已知z1求软体臂关节空间变量的函数，由公式(1)—式(9)表示。
2.根据权利要求1所述的适用于两段6自由度连续体机械臂的逆运动学求解方法，其特征在于，Err＝10-6。
CN202111338689.5A 2021-11-12 2021-11-12 适用于两段6自由度连续体机械臂的逆运动学求解方法 Active CN114012728B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111338689.5A CN114012728B (zh) 2021-11-12 2021-11-12 适用于两段6自由度连续体机械臂的逆运动学求解方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111338689.5A CN114012728B (zh) 2021-11-12 2021-11-12 适用于两段6自由度连续体机械臂的逆运动学求解方法

Publications (2)

Publication Number Publication Date
CN114012728A CN114012728A (zh) 2022-02-08
CN114012728B true CN114012728B (zh) 2023-04-25

Family

ID=80063988

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111338689.5A Active CN114012728B (zh) 2021-11-12 2021-11-12 适用于两段6自由度连续体机械臂的逆运动学求解方法

Country Status (1)

Country Link
CN (1) CN114012728B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101286115A (zh) * 2008-05-13 2008-10-15 北京邮电大学 六自由度机器人运动学cordic算法协处理器
CN104834809A (zh) * 2015-04-17 2015-08-12 中国石油大学(华东) 基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法
CN105050775A (zh) * 2013-03-15 2015-11-11 直观外科手术操作公司 软件可配置的操纵器自由度
CN109895101A (zh) * 2019-04-09 2019-06-18 大连理工大学 一种关节型机械臂逆运动学数值唯一解求取方法
CN110434851A (zh) * 2019-06-24 2019-11-12 浙江工业大学 一种5自由度机械臂逆运动学求解方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101286115A (zh) * 2008-05-13 2008-10-15 北京邮电大学 六自由度机器人运动学cordic算法协处理器
CN105050775A (zh) * 2013-03-15 2015-11-11 直观外科手术操作公司 软件可配置的操纵器自由度
CN104834809A (zh) * 2015-04-17 2015-08-12 中国石油大学(华东) 基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法
CN109895101A (zh) * 2019-04-09 2019-06-18 大连理工大学 一种关节型机械臂逆运动学数值唯一解求取方法
CN110434851A (zh) * 2019-06-24 2019-11-12 浙江工业大学 一种5自由度机械臂逆运动学求解方法

Also Published As

Publication number Publication date
CN114012728A (zh) 2022-02-08

Similar Documents

Publication Publication Date Title
CN108237534B (zh) 一种连续型机械臂的空间避障轨迹规划方法
CN106737689B (zh) 基于模式函数的超冗余机械臂混合逆向求解方法及系统
CN107891424B (zh) 求解冗余机械臂逆运动学的有限时间神经网络优化方法
JP4273335B2 (ja) ロボットアーム
US9248572B2 (en) Axis angle determination method for six-axis robot and control apparatus for six-axis robot
CN106844951B (zh) 基于分段几何法求解超冗余机器人逆运动学的方法及系统
CN110744547B (zh) 一种基于分段常曲率的连续体机械臂逆运动学建模方法
CN110370256B (zh) 机器人及其路径规划方法、装置和控制器
CN113146610B (zh) 基于零空间避障的机械臂末端轨迹跟踪算法
CN104866722B (zh) 一种七轴工业机械臂的逆运动学求解方法
CN109702751A (zh) 一种七自由度串联机械臂的位置级逆解方法
CN111660307A (zh) 一种机器人操作高辅精度的虚拟夹具控制方法及系统
TW202021752A (zh) 機械臂奇異點控制方法及系統
Li et al. A novel constrained tendon-driven serpentine manipulator
Andersson Discretization of a continuous curve
Zhang et al. Inverse kinematics of modular Cable-driven Snake-like Robots with flexible backbones
CN114012728B (zh) 适用于两段6自由度连续体机械臂的逆运动学求解方法
CN109311155B (zh) 一种工业机器人的工具坐标系原点的标定方法及装置
CN109434838B (zh) 线驱动连续机器人内窥操作的协同运动规划方法及系统
CN113334381B (zh) 一种可运动解耦的连续体机器人控制方法
Zhang et al. Design optimization of a cable-driven two-DOF joint module with a flexible backbone
CN110774286B (zh) 一种基于刚柔耦合动力学的五自由度机械手的控制方法
Li et al. Design and kinematics of a novel continuum robot connected by unique offset cross revolute joints
CN112917479A (zh) 五轴机器人的近似位姿计算方法、装置和存储介质
Wang et al. A novel 2-SUR 6-DOF parallel manipulator actuated by spherical motion generators

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