CN114888815A - 一种连续体机器人逆运动学求解方法 - Google Patents
一种连续体机器人逆运动学求解方法 Download PDFInfo
- Publication number
- CN114888815A CN114888815A CN202210781222.6A CN202210781222A CN114888815A CN 114888815 A CN114888815 A CN 114888815A CN 202210781222 A CN202210781222 A CN 202210781222A CN 114888815 A CN114888815 A CN 114888815A
- Authority
- CN
- China
- Prior art keywords
- continuum robot
- snake
- key node
- shaped arm
- pose
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 230000008569 process Effects 0.000 claims abstract description 11
- 238000007781 pre-processing Methods 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 abstract description 10
- 238000010586 diagram Methods 0.000 description 8
- 238000004088 simulation Methods 0.000 description 6
- 238000005452 bending Methods 0.000 description 4
- 230000007246 mechanism Effects 0.000 description 4
- WYTGDNHDOZPMIW-RCBQFDQVSA-N alstonine Natural products C1=CC2=C3C=CC=CC3=NC2=C2N1C[C@H]1[C@H](C)OC=C(C(=O)OC)[C@H]1C2 WYTGDNHDOZPMIW-RCBQFDQVSA-N 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 239000011664 nicotinic acid Substances 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 241000270295 Serpentes Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000002324 minimally invasive surgery Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
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/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
-
- 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/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
一种连续体机器人逆运动学求解方法,涉及连续体机器人运动控制领域,解决现有逆运动学方法存在着求解过程复杂,计算效率低,甚至存在奇异位形等问题,本发明通过位姿插值:蛇形臂关键节点预处理,蛇形臂关键节点逆向迭代和正向迭代计算以及判断误差值等步骤实现逆运动最终求解。本发明方法生成的位姿更加自然,连续体机器人的各个关节运动分布均匀。相较于雅可比矩阵的迭代式算法,本方法运动分布均匀,无奇异点,不存在奇异位姿等问题。将本发明所述的方法应用到连续体机器人的实时控制当中,可以提高运动控制的精度且实时性高,实现对连续体机器人的实时控制。
Description
技术领域
本发明涉及连续体机器人运动控制领域,具体涉及一种连续体机器人逆运动学求解方法。
背景技术
连续体机器人是一种新型的无关节的仿生机器人。连续体机器人相较于传统的刚性机器人而言,可以根据障碍物环境灵活的改变自身的形状,因此对狭小和非结构化的环境具有较强的适应能力。因而广泛的应用于外科微创手术,探索救援,航空检修等领域。连续体机器人的逆运动学是保障连续体机器人在狭小空间运行和作业的基础。
然而当前的逆运动学方法存在着求解过程复杂,计算效率低,甚至存在奇异位形等一系列问题。而快速可靠的逆运动学求解,是连续体机器人实时控制的基础。
为了解决上述问题,本发明的目的是提供一种简单有效的逆运动学求解方法,实现多段式连续体机器人快速有效的逆解。
发明内容
本发明为解决现有逆运动学方法存在着求解过程复杂,计算效率低,甚至存在奇异位形等问题,提供一种连续体机器人逆运动学求解方法。
一种连续体机器人逆运动学求解方法,该方法由以下步骤实现:
步骤一、位姿插值;
根据蛇形臂的初始位姿和目标位姿进行插值,获得蛇形臂路径上的中间目标点;
步骤二、蛇形臂关键节点预处理;
根据蛇形臂的当前位姿和中间目标点的位置,对蛇形臂的关键节点的位置进行调整;
步骤三、蛇形臂关键节点逆向迭代计算;
从蛇形臂的末端开始,根据上一个关键节点的位姿和下一个关键节点的坐标,逆向迭代求解蛇形臂的构型参数;
步骤四、蛇形臂关键节点正向迭代计算;
步骤五、判断误差;
根据蛇形臂的当前位姿和中间目标点的位置,判断蛇形臂末端距离中间目标点是否小于阈值,若否,则返回执行步骤三,若是,则执行步骤六;
步骤六、根据获得的蛇形臂的构型参数,蛇形臂运动到下一个目标点;
步骤七、判断蛇形臂是否到达路径终点,若是,则结束;若否,返回执行步骤二。
本发明的有益效果:
一、本发明所述的方法,将求解连续体机器人的逆运动学问题转化为关键节点的线性运算。同时通过启发式的正向迭代过程进一步减小了迭代过程的误差,优化了全局关键节点的位置,提高了算法的收敛速度。因此本方法的时间复杂度较低,运算效率高,可应用于连续体机器人的实时路径规划中,实现多段式连续体机器人的控制。
二、本发明所述的方法,无需设计复杂的控制器,控制收敛且性能稳定,因此该方法具有控制简单。
三、本发明所述的方法,生成的位姿更加自然。连续体机器人的各个关节运动分布均匀。相较于雅可比矩阵的迭代式算法,本方法运动分布均匀,无奇异点,不存在奇异位姿等问题。
四、将本发明所述的方法应用到连续体机器人的实时控制当中,可以提高运动控制的精度且实时性高,实现对连续体机器人的实时控制。
附图说明
图1为本发明所述的一种连续体机器人逆运动学求解方法中连续体机器人结构示意图;
图2为单段连续体机器人运动学分析示意图;
图3为多段连续体机器人运动学分析原理图;
图4为本发明所述的一种连续体机器人逆运动学求解方法的流程图;
图5为关键节点的预处理原理图;
图6为连续体机器人构型参数的求解原理图;
图7为连续体机器人关键节点的逆向迭代示意图;
图8为连续体机器人关键节点正向迭代示意图;
图9为连续体机器人直线轨迹路径跟踪仿真实验效果图;
图10为连续体机器人圆弧轨迹路径跟踪仿真实验效果图,(a)为连续体机器人圆弧轨迹跟随的前半部分(b)为连续体机器人圆弧轨迹跟随的后半部分。
具体实施方式
结合图1至图10说明本实施方式,一种连续体机器人的逆运动学求解方法,包括下列步骤:
S1、位姿插值:
设连续体机器人初始位姿为T0,连续体机器人目标位姿为T1,中间存在n个点,连续体机器人的位姿插值具体实现方式如下:
将齐次旋转矩阵转化为四元数,之后利用插值方法进行求解。设连续体机器人的当前位姿所对应的齐次旋转矩阵T为:
旋转矩阵T对应的四元数q0为:
q0=a0i+b0j+c0k+w0
式中,i、j、k为四元数虚数部分的三个单位,a0、b0、c0为四元数虚部的数值,w0为四元数实部的数值;
首先将齐次旋转矩阵转化为四元数,具体实施方式为:
设连续体机器人初始位姿T0和目标位姿T1对应的齐次旋转矩阵为:
q0=a0i+b0j+c0k+w0
q1=a1i+b1j+c1k+w1
引入中间变量t则第i个中间点对应的中间变量为:
两个四元数的夹角为:
设定阈值ε,若两个四元数的夹角小于ε,采用归一化线性插值的方法计算第i个中间点所对应的四元数。若两个四元数的夹角大于ε,则采用球面线性插值的方法进行插值。
若第i个中间点对应的齐次矩阵为Ti:
其中旋转矩阵Ri可以利用当前点处的四元数求得:
第i个中间点的坐标可以通过线性插值得到。p1(px1,py1,pz1)为连续体机器人初始位姿的坐标,p2(px2,py2,pz2)为连续体机器人目标位姿的坐标
S2、预处理,如图5所示。
在S2阶段中,根据连续体机器人当前关键节点的位置和中间目标点的位置对关键节点的位置进行调整,包括顺序执行下列的步骤:
步骤一:设连续体机器人目标点的坐标为pend,连续体机器人当前末端点的坐标为pini,将连续体机器人末端点移动到目标点。此时连续体机器人末端点移动的距离为:
dinv=pend-pini
步骤二:移动其他关键节点,作末端点到目标点的连线,其他关键节点的移动方向和末端点移动的方向一致。若连续体机器人共有n个关键节点,第i个关键节点的移动距离为:
S3、逆向迭代,如图6所示。
在步骤S3逆向迭代中,已知了第i个关键节点的位姿和第i-1个节点的坐标,求解第i段连续体机器人的具体方法如下:
设第i-1个关键节点的坐标为P(x1,y1,z1),第i个关键节点的坐标为O(x0,y0,z0),O点的局部坐标系{Ti},X轴的方向向量为nx,Y轴的方向向量为ny,Z轴的方向向量为nz,平面XOY的方程为:
nzx(x-x0)+nzy(y-y0)+nz(z-z0)=0
点P到平面XOY的距离为:
线段OP的长度为:
根据几何关系,可以求解得到第i段连续体机器人的弯曲角αi,当点P位于平面XOY上方时候,αi取正值,否则取负值。
设直线OP的方向向量为n1,易求得OP和Y轴正半轴的夹角θ的余弦值
根据三余弦定理,可以求出第i段连续体机器人的方向角θi,当点P位于平面YOZ上方是θi取负值,否则取正值。
本实施方式中,如图7所示,具有i段的连续体机器人逆向迭代,在步骤S3逆向迭代中,通过逆向迭代求解连续体机器人的构型参数,包括顺序执行以下步骤:
步骤一、根据第i+1个关键节点的位姿和第i个关键节点的坐标,求解第i段连续体机器人的构型参数。
步骤二、根据第i+1个关键节点的坐标和i段连续体机器人的构型参数,求解第i个关键节点的坐标
步骤三、重复上述步骤,直到求得第一段连续体机器人的构型参数
S4、正向迭代,如图8所示。
具有i段连续体机器人的正向迭代,在S4正向迭代步骤中通过正向迭代减小和目标点的误差,设连续体机器人第i个关键节点的坐标为pi,连续体机器人第i段的长度为l,具体过程如下:
首先,固定连续体机器人的末端关键节点,将连续体机器人的第一个关键节点移动到初始位置p1';第n个关键节点的坐标和第i个关键节点的坐标分别为Pn和Pi;第一个关键节点的坐标为P1;
移动其他关键节点。更新后的第i个关键节点位于第i个关键节点和第i-1个关键节点的连线上,移动距离取决于关键节点所处的相对位置,如下式所示:
其中
本实施方式中,如图1所示,对连续体机器人本体结构分析:
连续体机器人是一种具有较高的灵活性和较好柔顺性能的仿生机器人,一般由机器人本体1,驱动机构2,进给机构3组成。由于连续体机器人本体的结构类似于蜿蜒的蛇,因此又称之为蛇形臂。蛇形臂通常由多个关节段串联构成,每个关节段具有旋转和弯曲两个自由度。每个关节段的连接处为关键节点。进给机构具有一个自由度,可以实现一维的精确进给运动。驱动机构主要由电机和滑台构成,为连续体机器人实现驱动。
如图2所示,对单段连续体机器人的运动学分析;
第i段连续体机器人如图所示,连续体机器人和坐标轴的Z轴相切。连续体机器人单个关节的弯曲曲线可以近似的看成连续光滑的圆弧曲线。连续体机器人的单个关节的位姿可以用两个参数进行描述,弯曲角α和方向角θ,称之为连续体机器人的构型参数。
单个关键节点的坐标系{Ti}与{Ti-1}之间的坐标变换可以通过旋转矩阵得到,具体实现方式为:绕Z逆时针旋转θ角度,沿着Y轴平移li(1-cosαi)/αi,之后沿着Z轴平移lisinαi/αi,最后绕X轴顺时针旋转αi角度,如下式所示。
如图3所示,对多段连续体机器人运动学分析;
连续体机器人由多个关节串联而成,距离连续体机器人基座最近的关节为关键节点1,之后为关键节点2、关键节点3,连续体机器人末端为关节n;机器人基座坐标系{T1}和机器人末端的坐标系{Tn}之间的坐标变换关系如下式所示:
如图9和图10所示,对连续体机器人仿真实验:为了验证本实施方式提出的连续体机器人逆运动学求解方法,进行了以下实验:
首先,对连续体机器人直线轨迹逆运动学仿真实验:
连续体机器人直线轨迹逆运动学仿真实验如图9所示,连续体机器人初始位姿为位姿1,连续体机器人末端通过直线轨迹经过中间点B点到达最终目标点C点。连续体机器人在整个运动过程中运动平稳,连续性较高,没有位姿和构型的突变。
其次,对连续体机器人圆弧轨迹逆运动学仿真实验:
连续体机器人圆弧轨迹逆运动学仿真如图10所示,连续体机器人末端运动轨迹为圆弧,通过逆运动学求解得到了连续体机器人的构型参数。在整个过程中,连续体机器人运动分布均匀,没有不连续的突变。
通过以上实验可以充分的证明本实施方式所提出的连续体机器人逆运动学方法能够实现连续体机器人逆运动学的求解,具有求解简单,时间复杂度低和运动分布均匀等特点。
以上所述实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本发明的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。因此,本发明专利的保护范围应以所附权利要求为准。
Claims (4)
1.一种连续体机器人逆运动学求解方法,其特征是:该方法由以下步骤实现:
步骤一、位姿插值;
根据蛇形臂的初始位姿和目标位姿进行插值,获得蛇形臂路径上的中间目标点;
步骤二、蛇形臂关键节点预处理;
根据蛇形臂的当前位姿和中间目标点的位置,对蛇形臂的关键节点的位置进行调整;
步骤三、蛇形臂关键节点逆向迭代计算;
从蛇形臂的末端开始,根据上一个关键节点的位姿和下一个关键节点的坐标,逆向迭代求解蛇形臂的构型参数;
步骤四、蛇形臂关键节点正向迭代计算;
步骤五、判断误差;
根据蛇形臂的当前位姿和中间目标点的位置,判断蛇形臂末端距离中间目标点是否小于阈值,若否,则返回执行步骤三,若是,则执行步骤六;
步骤六、根据获得的蛇形臂的构型参数,蛇形臂运动到下一个目标点;
步骤七、判断蛇形臂是否到达路径终点,若是,则结束;若否,返回执行步骤二。
3.根据权利要求1所述的一种连续体机器人逆运动学求解方法,其特征在于:步骤三中蛇形臂关键节点逆向迭代计算的过程为:
步骤一、根据第i+1个关键节点的位姿和第i个关键节点的坐标,求解第i段连续体机器人的构型参数;
步骤二、根据第i+1个关键节点的坐标和i段连续体机器人的构型参数,求解第i个关键节点的坐标;
步骤三、返回执行步骤一,直到求得第一段连续体机器人的构型参数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210781222.6A CN114888815A (zh) | 2022-07-04 | 2022-07-04 | 一种连续体机器人逆运动学求解方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210781222.6A CN114888815A (zh) | 2022-07-04 | 2022-07-04 | 一种连续体机器人逆运动学求解方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114888815A true CN114888815A (zh) | 2022-08-12 |
Family
ID=82729738
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210781222.6A Pending CN114888815A (zh) | 2022-07-04 | 2022-07-04 | 一种连续体机器人逆运动学求解方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114888815A (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018133439A1 (zh) * | 2017-01-19 | 2018-07-26 | 北京工业大学 | 一种考虑末端运动误差的六自由度机器人轨迹规划方法 |
CN108673509A (zh) * | 2018-06-15 | 2018-10-19 | 东北大学 | 一种六自由度手腕偏置型串联机械臂的运动控制方法 |
CN108972558A (zh) * | 2018-08-16 | 2018-12-11 | 居鹤华 | 一种基于轴不变量的多轴机器人动力学建模方法 |
CN113334381A (zh) * | 2021-06-01 | 2021-09-03 | 中国科学院沈阳自动化研究所 | 一种可运动解耦的连续体机器人控制方法 |
US20220009096A1 (en) * | 2020-07-10 | 2022-01-13 | Ubtech Robotics Corp Ltd | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same |
CN114147720A (zh) * | 2021-12-14 | 2022-03-08 | 杭州锐沃机器人科技有限公司 | 一种多自由度机械臂的逆运动学通用求解方法及装置 |
-
2022
- 2022-07-04 CN CN202210781222.6A patent/CN114888815A/zh active Pending
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018133439A1 (zh) * | 2017-01-19 | 2018-07-26 | 北京工业大学 | 一种考虑末端运动误差的六自由度机器人轨迹规划方法 |
CN108673509A (zh) * | 2018-06-15 | 2018-10-19 | 东北大学 | 一种六自由度手腕偏置型串联机械臂的运动控制方法 |
CN108972558A (zh) * | 2018-08-16 | 2018-12-11 | 居鹤华 | 一种基于轴不变量的多轴机器人动力学建模方法 |
US20220009096A1 (en) * | 2020-07-10 | 2022-01-13 | Ubtech Robotics Corp Ltd | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same |
CN113334381A (zh) * | 2021-06-01 | 2021-09-03 | 中国科学院沈阳自动化研究所 | 一种可运动解耦的连续体机器人控制方法 |
CN114147720A (zh) * | 2021-12-14 | 2022-03-08 | 杭州锐沃机器人科技有限公司 | 一种多自由度机械臂的逆运动学通用求解方法及装置 |
Non-Patent Citations (2)
Title |
---|
房立金;高瑞;: "一般6R机器人逆运动学算法的改进", 机械科学与技术, no. 09, pages 1325 - 1330 * |
曹晟阁: "滚动接触柔性连续体机器人的设计与运动能力分析", 机械工程学报, vol. 57, no. 19, pages 21 - 29 * |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108237534B (zh) | 一种连续型机械臂的空间避障轨迹规划方法 | |
CN111300425B (zh) | 一种超冗余机械臂末端轨迹运动规划方法 | |
CN112091979B (zh) | 一种基于位置级逆运动学的七自由度机械臂限位优化方法 | |
US11845186B2 (en) | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same | |
CN106625666B (zh) | 冗余机械臂的控制方法及装置 | |
Qi et al. | Kinematic control of continuum manipulators using a fuzzy-model-based approach | |
CN109397271B (zh) | 一种7自由度拟人机械臂及其控制方法和系统 | |
CN106737689B (zh) | 基于模式函数的超冗余机械臂混合逆向求解方法及系统 | |
CN106844951B (zh) | 基于分段几何法求解超冗余机器人逆运动学的方法及系统 | |
Chitrakaran et al. | Setpoint regulation of continuum robots using a fixed camera | |
CN104933232B (zh) | 一种杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法 | |
CN111123943B (zh) | 一种基于伪逆约束的超冗余机器人轨迹规划方法及系统 | |
CN113146600A (zh) | 基于运动学迭代学习控制的柔性机器人轨迹规划方法及装置 | |
Qin et al. | Adaptive trajectory control of an under-actuated snake robot | |
Antonov et al. | Position, velocity, and workspace analysis of a novel 6-DOF parallel manipulator with “piercing” rods | |
CN113334381B (zh) | 一种可运动解耦的连续体机器人控制方法 | |
Luo et al. | A bioinspired coiled cable-driven manipulator: Mechatronic design and kinematics planning with multiconstraints | |
CN113878571A (zh) | 一种七自由度协作机器人构型优化综合方法 | |
Wang et al. | Design and analysis of a snake-like surgical robot with continuum joints | |
CN114888815A (zh) | 一种连续体机器人逆运动学求解方法 | |
Ren et al. | Knowledge database-based multiobjective trajectory planning of 7-DOF manipulator with rapid and continuous response to uncertain fast-flying objects | |
Chen et al. | Minimum sweeping area motion planning for flexible serpentine surgical manipulator with kinematic constraints | |
CN115061475A (zh) | 一种基于导航路径约束的柔性机器人路径规划方法 | |
CN105629880B (zh) | 用于具有三角形远心点机构的串联机械臂逆运动学数值解法 | |
Niu et al. | Shape-controllable inverse kinematics of hyper-redundant robots based on the improved FABRIK method |
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 | ||
AD01 | Patent right deemed abandoned |
Effective date of abandoning: 20240802 |
|
AD01 | Patent right deemed abandoned |