一种基于分段常曲率的连续体机械臂逆运动学建模方法与流程

本发明涉及机械臂控制技术领域，尤其涉及一种基于分段常曲率的连续体机械臂逆运动学建模方法。

背景技术：

柔性机械臂的多冗余度特征使其较传统离散机械臂具有更为良好的灵巧性，因而被广泛地应用到工业和医疗等相关领域，柔性机械臂一般可分为两种：软体与连续体。软体机械臂大多是利用仿生技术，从行为或功能上模仿自然界中的生物，如章鱼触手、大象鼻子、尺蠖等,一般多由硅胶、橡胶等弹性材料制成，其理论上具有无限自由度。连续体机械臂通常是将若个相同或相似单元两两串联，或者是在管状材质侧壁切割出切槽，与软体机械臂相比，其具有有限个自由度，但承受负载能力、可操纵性优于软体机械臂。连续体机械臂的多冗余度特征使其较传统离散机械臂获得了优良灵活性，但同时也导致其正逆运动学求解变得困难，制约着自身在相关领域的进一步发展。

运动学模型旨在建立起驱动丝长度与连续体机械臂末端位姿之间的相互映射关系。如图1所示，建立了驱动空间、形态空间和工作空间，驱动空间包含驱动丝长度l，形态空间包含机械臂弯曲角度θ和弯曲方向φ，工作空间包含机械臂末端位置和姿态q。逆运动学映射建立过程均分两步进行，映射建立顺序为形态空间-驱动空间，工作空间-形态空间。

发明人发现，许多学者采用不同方法对连续体机械臂的运动学问题进行了深入研究，如模态方法根据机械臂末端位置将机械臂宏观几何特征抽象为等曲率变化的骨干曲线，通过建立形函数实现了对逆运动学的求解；如针对切口式连续体机械臂基于悬臂梁理论建立了驱动力与机械臂末端位置之间的正逆运动学模型；如利用椭圆积分给出了多骨干连续体机械臂瞬时正运动学解析公式，建立了形态空间到工作空间的映射关系；如基于雅可比矩阵求解连续体机械臂逆运动学的方法等等，这些传统方法主要存在以下问题：一是只解决正运动学问题，没有考虑逆运动学问题；二是虽解决了正逆运动学问题，但没有推导出逆运动学解析解；三是运动学模型计算效率低，不满足机械臂实时控制要求；四是没有考虑机械臂末端姿态问题。

技术实现要素：

为了克服现有技术的不足，本发明提供了一种基于分段常曲率的连续体机械臂逆运动学建模方法，基于分段常曲率假设，通过提取机械臂骨干曲线建立机械臂逆运动学求解几何模型；通过引入四元数，给出了连续体机械臂末端位姿的约束条件；建立起了包含末端位置和姿态的逆运动学模型，并推导出了解析解。

本发明采用下述技术方案：

一种基于分段常曲率的连续体机械臂逆运动学建模方法，包括以下步骤：

步骤(1)基于分段常曲率假设，建立机械臂关节的几何模型，得出机械臂弯曲方向和弯曲角度到驱动丝长度的映射关系；

步骤(2)提取机械臂骨干曲线模型，建立机械臂末端位置到机械臂弯曲方向和弯曲角度的映射关系；

步骤(3)基于四元数得出机械臂末端位姿的约束条件，建立包含机械臂末端位置和姿态的逆运动学模型，并得出解析解。

进一步的，连续体机械臂通过两组驱动丝控制其弯曲运动，两组驱动丝的长度表示为：

其中，φ∈[0,2π)

d表示驱动丝所在圆周直径，n表示机械臂关节个数，p1、p3与p2、p4分别表示两组驱动丝位置，θ表示机械臂弯曲角度，h表示关节无偏转时两个单元之间的驱动丝长度，c为常数。

进一步的，c＝nh0+hb+he，其中，hb表示机械臂基座高度，he表示机械臂末端单元高度，h0表示单元高度。

进一步的，其中，hb和db表示单元的倒角尺寸，h表示单元内驱动丝长度，d表示单元的最大直径。

进一步的，机械臂末端位姿q＝(x,y,z,α,β,γ)^t，其中x、y、z表示末端位置的笛卡尔坐标系三维坐标，α、β、γ表示末端姿态的欧拉角。

进一步的，其中，c0＝he-hb-h。

进一步的，根据四元数性质得到约束条件：

其中，α、β、γ表示机械臂末端姿态。

进一步的，给定满足约束条件的末端位姿q可得到驱动丝长度。

与现有技术相比，本发明的有益效果是：

(1)本发明解决了传统针对分段常曲率线驱连续体机械臂的逆运动学模型不能得到解析解，也没有考虑机械臂末端姿态的问题，为连续体机械臂提供了一种新的建模思路，利用该方法可实现连续体机械臂位置或姿态的低延时控制；

(2)本发明基于分段常曲率假设，通过提取机械臂骨干曲线建立机械臂逆运动学求解几何模型；通过引入四元数，给出了连续体机械臂末端位姿的约束条件；建立起了包含末端位置和姿态的逆运动学模型，并推导出了解析解。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1为正逆运动学映射示意图；

图2为本发明实施例一的连续体机械臂结构示意图；

图3为本发明实施例一的连续体机械臂侧视图；

图4为本发明实施例一的连续体机械臂的关节连接示意图；

图5为本发明实施例一的无偏转时连续体机械臂关节示意图；

图6为本发明实施例一的偏转θ角时连续体机械臂关节示意图；

图7为本发明实施例一的连续体机械臂截面示意图；

图8-图9为本发明实施例一的连续体机械臂骨干曲线模型示意图；

图10为本发明实施例一的引入四元数后机械臂骨干曲线模型示意图；

其中，1、基座，2、关节，3、中间单元，4、橡胶管，5、驱动丝，6、手术器械，7、末端单元。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合；

正如背景技术所介绍的，现有技术中存在传统针对分段常曲率线驱连续体机械臂的逆运动学模型不能得到解析解，没有考虑机械臂末端姿态的不足，为了解决如上的技术问题，本发明提出了一种基于分段常曲率的连续体机械臂逆运动学建模方法。

实施例一：

下面结合附图2-图10对本发明进行详细说明，具体的，结构如下：

连续体机械臂结构如图2所示，具有三个自由度。连续机械臂主体连接手术器械6，连续机械臂主体包括若干个中间单元3、关节2、基座1、末端单元7和橡胶管4，橡胶管4作为骨架将所有单元依次串联起来，每两个单元之间通过球面接触形成一个关节，关节的偏转由驱动丝5收放变化来控制，偏转叠加引起机械臂整体的弯曲运动。四根驱动丝5均匀分布在同一圆周上，中心对称的两根驱动丝5收放可控制机械臂在平面内做正向和反向弯曲运动，四根驱动丝5同时收放可实现机械臂在xy平面内任意方向的弯曲运动。

本实施例基于分段常曲率的连续体机械臂逆运动学建模方法，首先建立机械臂关节的几何模型，得出机械臂弯曲方向和弯曲角度到驱动丝长度的映射关系；然后通过提取机械臂骨干曲线模型，建立机械臂末端位置到机械臂弯曲方向和弯曲角度的映射关系；通过引入四元数，给出连续体机械臂末端位姿的约束条件；最终建立起包含末端位置和姿态的逆运动学模型，并推导出解析解。

具体的，

一、形态空间-驱动空间：

连续体机械臂关节由两个相邻单元通过球面接触构成，如图5所示，参数d表示单元的最大直径，d表示驱动丝所在圆周直径，h表示单元内驱动丝长度，h0表示单元高度，hb和db表示单元的倒角尺寸，h表示关节无偏转时两个单元之间的驱动丝长度，h0表示关节偏转时两个单元之间的驱动丝间距，由相似原理可知：

如图6所示，当关节在驱动丝平面内偏转θ角度时，根据几何关系可推出：

其中，hl和hr为两个单元间成中心对称的两根驱动丝长度。

假设连续体机械臂具有n个关节，根据分段常曲率原理，每个关节偏转角度相等，均为θ，则可得到：

ll＝nhl+c

lr＝nhr+c(4)

其中c＝nh0+hb+he，其始终为常数，ll和lr为机械臂内一组驱动丝长度，hb表示机械臂基座高度，he表示机械臂末端单元高度。

令由公式(3)(4)可推出：

关节的偏转导致机械臂的弯曲运动，机械臂弯曲角度θ表示为：

θ＝nθ(6)

机械臂弯曲角度存在最大值，该值由关节的单元自身参数决定：

连续体机械臂通过两组驱动丝控制其弯曲运动，如图7所示，p1、p3与p2、p4分别表示两组驱动丝位置，假设机械臂沿与x轴夹角为φ的方向弯曲，φ∈[0,2π)，每个关节偏转角度为θ，p1^e、p2^e、p3^e、p4^e分别为p1、p2、p3、p4在虚轴x＇上的等效点，则机械臂内两组驱动丝的长度表示为：

其中

特别地，当φ＝0时，机械臂沿x轴正向弯曲，

此时大小相同，且随θ的变化而变化。

二、工作空间-形态空间

机械臂末端位姿可用q＝(x,y,z,α,β,γ)^t表示，其中x、y、z是表征末端位置的笛卡尔坐标系三维坐标，α、β、γ是表征末端姿态的rpy(旋转次序x-y-z)欧拉角。

如图8和图9所示，当根据三角几何关系可得到：

把公式(12)代入公式(13)，并舍去多余解，可得到

其中，c0＝he-hb-h。

当基于上述方法仍然可得到公式(10)(11)(14)，此处不再赘述。

连续体机械臂具有三个自由度，因此必定存在约束条件限制末端位姿q。如图10所示，坐标系{b}到{e}的变换过程，又可理解为先做绕单位向量r＝(-sinφ,cosφ,0)的旋转变换r(θ,r)，再做沿l的平移变换。r(θ,r)可用单位四元数q＝η+uib+vjb+wkb唯一表示，并且有

其中ib、jb、kb为坐标系{b}的三个基向量。将公式(11)(14)带入公式(15)，可求得用末端位置坐标x，y，z表示q的表达式：

由四元数性质可知表征机械臂末端姿态的α、β、γ与四元数q具有如下关系：

结合式(17)(18)可得到约束条件：

结合式(10)(15)(18)可得到约束条件的另外一种表达形式

x＝f(α，β，γ)

y＝g(α，β，γ)

z＝h(α，β，γ)(20)

连续体机械臂逆运动学映射建立完成，得到了如公式(8)(10)(14)(19)(20)的逆运动学解析解，给定满足约束条件(19)或(20)的末端位姿q即可求解驱动丝长度。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。