CN110125933B - 软体机械臂的力学模型的建立方法及控制方法 - Google Patents

软体机械臂的力学模型的建立方法及控制方法 Download PDF

Info

Publication number
CN110125933B
CN110125933B CN201910386781.5A CN201910386781A CN110125933B CN 110125933 B CN110125933 B CN 110125933B CN 201910386781 A CN201910386781 A CN 201910386781A CN 110125933 B CN110125933 B CN 110125933B
Authority
CN
China
Prior art keywords
soft
driver
coordinate system
bottom end
qth
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
CN201910386781.5A
Other languages
English (en)
Other versions
CN110125933A (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.)
National Defense Technology Innovation Institute PLA Academy of Military Science
Original Assignee
National Defense Technology Innovation Institute PLA Academy of Military Science
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 National Defense Technology Innovation Institute PLA Academy of Military Science filed Critical National Defense Technology Innovation Institute PLA Academy of Military Science
Priority to CN201910386781.5A priority Critical patent/CN110125933B/zh
Publication of CN110125933A publication Critical patent/CN110125933A/zh
Application granted granted Critical
Publication of CN110125933B publication Critical patent/CN110125933B/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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • 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/1635—Programme controls characterised by the control loop flexible-arm control

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Aerodynamic Tests, Hydrodynamic Tests, Wind Tunnels, And Water Tanks (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种软体机械臂的力学模型的建立方法及控制方法，包括以下步骤：在每个软体驱动器的底端建立三维坐标系，第一个软体驱动器底端的坐标系为固定底端坐标系；从M个软体驱动器中任取一个作为第q个软体驱动器，从第q个软体驱动器的底端依次确定第i个特征点在第q个软体驱动器底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立第q个软体驱动器的力学模型，以相同方式分别建立M个软体驱动器的力学模型；将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中；根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，建立软体机械臂的力学模型。

Description

软体机械臂的力学模型的建立方法及控制方法
技术领域
本发明涉及软体机械臂技术领域，特别地，涉及一种软体机械臂的力学模型的建立方法，此外，还涉及软体机械臂空间运动姿态的开环控制方法和闭环控制方法。
背景技术
软体机械臂的研究主要受到自然界中的软体动物(如章鱼、象鼻等)的运动模式启发，通过软性材料搭配气动控制系统，实现超冗余和无限自由度的柔顺运动。软体机械臂因其柔性结构特性，使其在复杂未知环境探测、人机友好交互等方面，都具有刚性机械臂难以匹敌的优势。现有的软体机械臂通过控制气囊腔的充气和放气以实现软体机械臂的运动，但由于其结构的柔性，导致控制精度很难保证，并且负载能力差，无法负载质量大的物体。对于一种气控型刚柔耦合模块化的软体机械臂，通过将多个波纹管状的气囊体沿周向等距排布构成软体驱动器，软体驱动器上设有用于沿径向拉结多个气囊体的固定结构，多个固定结构沿气囊模块轴向间隔排布，用于提高机械臂的负载能力并使软体驱动器的所有气囊体相互做耦合运动，再通过连接结构将多个软体驱动器沿轴向首尾密封连接，气囊体充气后膨胀延长，气囊体放气后收缩缩短，通过向气囊体内充气和/或放气以控制气囊体中压力值，从而控制软体驱动器的空间运动姿态，目前可采用开环控制方法对软体机械臂的空间运动姿态进行控制，通过向各个气囊体中对应输入准确的压力值，以使软体机械臂形成指定的运动姿态，还可采用闭环控制方法对软体机械臂的空间运动姿态进行控制，通过向各个气囊体中输入初始的压力值后使软体机械臂形成运动姿态，再通过将运动姿态与指定运动姿态进行比较后对输入气囊体中的压力值进行调整，直至软体机械臂形成指定运动姿态，但由于目前无法对运动姿态进行准确计算，导致软体机械臂空间运动姿态的开环控制和闭环控制均难以实现。
发明内容
本发明提供了一种软体机械臂的力学模型的建立方法及控制方法，以解决气控型刚柔耦合模块化的软体机械臂的空间运动姿态无法精确控制的技术问题。
根据本发明的一个方面，提供一种软体机械臂的力学模型的建立方法，用于获取软体机械臂上空间位置和空间姿态以对软体机械臂的运动进行控制，软体机械臂包括M个软体驱动器，软体驱动器通过连接结构沿轴向首尾密封连接，软体驱动器包括沿周向等距排布的多个波纹管状的气囊体，每个气囊体包括多个波纹小节，波纹小节的分布中心为特征点，M个软体驱动器从软体机械臂的固定端向自由端端方向依次排列为第一个软体驱动器，第二个软体驱动器，……，第M个软体驱动器；M个软体驱动器上波纹小节的个数依次为N1，N2，……，NM；M个软体驱动器1上特征点的个数依次为N1，N2，……，NM，软体驱动器上靠近软体机械臂固定端的一端为底端，在每个软体驱动器的底端建立B-xyz三维坐标系，位于软体机械臂固定端处的第一个软体驱动器的坐标系为固定底端坐标系；从M个软体驱动器中任取一个作为第q个软体驱动器，从第q个软体驱动器的底端依次确定第i(1≤i≤Nq)个特征点在第q个软体驱动器底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立第q个软体驱动器的力学模型，以相同方式分别建立M个软体驱动器的力学模型；将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中；根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型。
软体驱动器的底端建立的坐标系，以软体驱动器中一个气囊体位于底端的波纹小节的中心点为坐标原点B。
确定第q个软体驱动器的第i个特征点在第q个软体驱动器的底端的坐标系中的弯曲方向和弯曲角度，包括以下步骤：分别根据第q个软体驱动器中波纹小节的弹性系数、气囊体内腔的横截面积、波纹小节的原始长度以及不同气囊体中的气压值，分别确定第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度；根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度，确定第q个软体驱动器的第i个特征点处的弯曲方向；根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度、气囊体壁体的横截面积、波纹小节的原始长度、气囊体之间的间距、气囊体的惯性距以及波纹小节的弹性系数，确定第q个软体驱动器的第i个特征点处的弯曲角度。
确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标，包括以下步骤：根据第q个软体驱动器的第i个特征点的弯曲方向、波纹小节的原始长度、波纹小节的弹性系数以及气囊体壁体的横截面积，分别计算第q个软体驱动器的不同气囊体的第i个波纹小节的中心点在第q个软体驱动器的底端的坐标系中的位置矢量；根据第q个软体驱动器的不同气囊体的第i个波纹小节的中心点的位置矢量以及第q个软体驱动器的前(i-1)个特征点的位置矢量在第q个软体驱动器的底端的坐标系中旋转矩阵，确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标。
将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中，包括以下步骤：当软体驱动器排第一时，则无需转换；当软体驱动器排第二至M时，则将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到前一个软体驱动器的底端的坐标系中，依次类推，将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中。
特征点在固定底端坐标系中的空间姿态采用固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角表示。
获得特征点在固定底端坐标系中的空间姿态，包括以下步骤：确定特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系；根据特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系，确定固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角。
当确定的特征点为第q个软体驱动器上第i个特征点，确定第q个软体驱动器上第i个特征点在第q个软体驱动器底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系，包括以下步骤：第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标为(xq,i yq,i zq,i)T，第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标为(x0 y0 z0)T，第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系为：
Figure GDA0002524105770000031
Hu(αu,θu)＝Mz(αu)My(θu)Mz(-αu)，u＝1，2，……，(q-1)，
Figure GDA0002524105770000033
Figure GDA0002524105770000032
其中，
αu为第u个软体驱动器的弯曲方向，θu为第u个软体驱动器的弯曲角度，
Mz(αu)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转αu的旋转矩阵，
My(θu)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕y轴旋转θu的旋转矩阵，
Mz(-αu)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转-αu的旋转矩阵，
Rz(α0)Ry(β0)Rx(γ0)为固定底端坐标系按照3-2-1顺序分别旋转欧拉角α0、β0、γ0后与第q个软体驱动器的底端的坐标系重合的旋转矩阵。
根据本发明的另一方面，还提供一种软体机械臂空间运动姿态的开环控制方法，包括以下步骤：采用上述软体机械臂的力学模型对软体机械臂的指定的空间运动姿态进行模拟，从而确定输入软体机械臂的各个气囊体中的气压值；分别将确定的气压值对应输入软体机械臂的各个气囊体中，使软体机械臂形成指定的空间运动姿态。
根据本发明的另一方面，还提供一种软体机械臂空间运动姿态的闭环控制方法，包括以下步骤：分别向软体机械臂的各个气囊体输入初始的压力值，使软体机械臂在空间运动；采用上述软体机械臂的力学模型确定软体机械臂的空间位置和空间姿态；根据软体机械臂的空间位置和空间姿态与指定的空间运动姿态之间的差异对输入气囊体中的压力值进行调整，直至软体机械臂形成的空间运动姿态为指定的空间运动姿态。
本发明具有以下有益效果：
本发明的软体机械臂的力学模型的建立方法，首先在每个软体驱动器的底端建立B-xyz三维坐标系，由于位于软体机械臂固定端处的第一个软体驱动器的底端在软体机械臂的运动过程中位置固定，因此为固定底端坐标系，通过以波纹管状的气囊体的伸长变形的线弹性假设和弯曲变形的弹性梁假设为基础，从软体驱动器的底端依次确定软体驱动器上的特征点在所在的软体驱动的底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立软体驱动器的力学模型，由于除了位于软体机械臂固定端处的第一个软体驱动器的底端的位置固定不变，软体机械臂的其他部位的位置均会在运动过程中发生改变，因此通过将特征点在自身所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中，以固定底端坐标系作为软体机械臂的定位基准，最后根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型。确定了输入气囊体的压力值、软体机械臂位置、空间运动姿态之间的关系，一方面可通过软体机械臂的力学模型模拟出软体机械臂的指定的空间运动状态，从而确定软体机械臂形成指定的空间运动状态时输入各个气囊体中的气压值，再将确定的气压值对应输入软体机械臂的各个气囊体中，使软体机械臂形成指定的空间运动姿态，从而实现对软体机械臂的开环控制；另一方面可通过分别向软体机械臂的各个气囊体输入初始的压力值，使软体机械臂在空间运动，采用软体机械臂的力学模型确定软体机械臂的空间位置和空间姿态，再根据软体机械臂的空间位置和空间姿态与指定的空间运动姿态之间的差异对输入气囊体中的压力值进行调整，直至软体机械臂形成的空间运动姿态为指定的空间运动姿态，从而实现对软体机械臂的精确闭环控制。
除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是本发明优选实施例的软体机械臂的结构示意图；
图2是本发明优选实施例的软体机械臂的力学模型的建立方法的流程示意图。
图例说明：
1、软体驱动器；11、气囊体；2、连接结构。
具体实施方式
以下结合附图对本发明的实施例进行详细说明，但是本发明可以由下述所限定和覆盖的多种不同方式实施。
图1是本发明优选实施例的软体机械臂的结构示意图；图2是本发明优选实施例的软体机械臂的力学模型的流程示意图。
如图1和图2所示，本实施例的软体机械臂的力学模型的建立方法，用于获取软体机械臂上空间位置和空间姿态以对软体机械臂的运动进行控制，软体机械臂包括M个软体驱动器1，软体驱动器1通过连接结构2沿轴向首尾密封连接，软体驱动器1包括沿周向等距排布的多个波纹管状的气囊体11，每个气囊体11包括多个波纹小节，波纹小节的分布中心为特征点，M个软体驱动器1从软体机械臂的固定端向自由端依次排列为第一个软体驱动器，第二个软体驱动器，……，第M个软体驱动器；M个软体驱动器上波纹小节的个数依次为N1，N2，……，NM；M个软体驱动器1上特征点的个数依次为N1，N2，……，NM，建立方法包括以下步骤：软体驱动器上靠近软体机械臂固定端的一端为底端，在每个软体驱动器的底端建立B-xyz三维坐标系，位于软体机械臂固定端处的第一个软体驱动器的坐标系为固定底端坐标系；从M个软体驱动器中任取一个作为第q个软体驱动器，从第q个软体驱动器的底端依次确定第i(1≤i≤Nq)个特征点在第q个软体驱动器的底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立第q个软体驱动器的力学模型，以相同方式分别建立M个软体驱动器的力学模型；将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中；根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型。本发明的软体机械臂的力学模型，首先在每个软体驱动器的底端建立B-xyz三维坐标系，由于位于软体机械臂固定端处的第一个软体驱动器的底端在软体机械臂的运动过程中位置固定，因此为固定底端坐标系，通过以波纹管状的气囊体的伸长变形的线弹性假设和弯曲变形的弹性梁假设为基础，从软体驱动器的底端依次确定软体驱动器上的特征点在所在的软体驱动的底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立软体驱动器的力学模型，由于除了位于软体机械臂固定端处的第一个软体驱动器的底端的位置固定不变，软体机械臂的其他部位的位置均会在运动过程中发生改变，因此通过将特征点在自身所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中，以固定底端坐标系作为软体机械臂的定位基准，最后根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型，确定了输入气囊体的压力值、软体机械臂位置、空间运动姿态之间的关系。
软体驱动器的底端建立坐标系，以软体驱动器中一个气囊体位于底端的波纹小节的中心点为坐标原点B。由于特征点为气囊体上同节的波纹小节的分布中心，而特征点周围的波纹小节的分布位置不相同，为了更加准确地获得特征点的位置坐标，需要获得周围的波纹小节的中心点的位置坐标，通过以软体驱动器中一个气囊体位于底端的波纹小节的中心点为坐标原点B，使得该软体驱动器上所有的波纹小节的中心点的位置坐标易于确定。
确定第q个软体驱动器的第i个特征点在第q个软体驱动器的底端的坐标系中的弯曲方向和弯曲角度，包括以下步骤：分别根据第q个软体驱动器中波纹小节的弹性系数、气囊体内腔的横截面积、波纹小节的原始长度以及不同气囊体中的气压值，分别确定第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度；根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度，确定第q个软体驱动器的第i个特征点处的弯曲方向；根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度、气囊体壁体的横截面积、波纹小节的原始长度、气囊体之间的间距、气囊体的惯性距以及波纹小节的弹性系数，确定第q个软体驱动器的第i个特征点处的弯曲角度。
确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标，包括以下步骤：根据第q个软体驱动器的第i个特征点的弯曲方向、波纹小节的原始长度、波纹小节的弹性系数以及气囊体壁体的横截面积，分别计算第q个软体驱动器的不同气囊体的第i个波纹小节的中心点在第q个软体驱动器的底端的坐标系中的位置矢量；根据第q个软体驱动器的不同气囊体的第i个波纹小节的中心点的位置矢量以及第q个软体驱动器的第1～(i-1)个特征点的位置矢量在第q个软体驱动器的底端的坐标系中旋转矩阵，确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标。
在本实施例中，每个软体驱动器包括沿周向等距布设的三个气囊体。以建立第一个软体驱动器的力学模型为例，第一个软体驱动器上包括N1个特征点，从第1个软体驱动器的底端依次确定第i(1≤i≤N1)个特征点在第一个软体驱动器的底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，包括以下步骤：
确定第一个软体驱动器中三个气囊体上第i个波纹小节与气囊体底端之间的长度：
Figure GDA0002524105770000061
确定第一个软体驱动器的第i个特征点处的弯曲方向，弯曲方向为软体驱动器第i个特征点处从X轴沿逆时针方向转动的角度α1：
Figure GDA0002524105770000062
且α1与L11、L12以及L13之间需满足以下关系：
Figure GDA0002524105770000071
其中，L11、L12以及L13分别为第一个软体驱动器中三个气囊体第i个波纹小节与气囊体底端之间的长度；
确定第一个软体驱动器第i个特征点处的弯曲角度θ1，软体驱动器假设为弹性梁，弯曲角度为弹性梁第i个特征点处的转角：
Figure GDA0002524105770000072
S′1＝π(rout,1 2-rin,1 2)，
I1为第一个软体驱动器中气囊体截面的惯性矩，
Figure GDA0002524105770000073
W1的计算方法为：
当L111L13时，
Figure GDA0002524105770000074
当L11＝L13≠L12时，
Figure GDA0002524105770000075
根W1中包含两个正数解，取其较小者为方程的实际解。
当L11＝L12＝L13时，W1＝0，
其中，S1为第一个软体驱动器中气囊体内腔的横截面积，S1‘为第一个软体驱动器中气囊体壁体的横截面积，a1为气囊体之间的间距，rout,1为第一个软体驱动器中气囊体的外径，rin,1为第一个软体驱动器中气囊体的内径，k1为第一个软体驱动器中气囊体中的弹性系数；
三个气囊体第i个波纹小节的中心点分别为A’、B’、C’，其中B’与第一个软体驱动器的底端的坐标系的原点位于同一个气囊体中，确定第一个软体驱动器的底端的坐标系的原点分别到与第i个特征点同节的三个气囊体的第i个波纹小节的中心点的位置矢量r1,A′、r1,B′、r1,C′：
Figure GDA0002524105770000081
其中，P11、P12、P13分别为A’、B’、C’所在的气囊体的压力值，L01为第一个软体驱动器中波纹小节的原始长度；
确定第一个软体驱动器上第i个特征点在第一个软体驱动器的底端的坐标系中的位置矢量r1,i：
Figure GDA0002524105770000082
其中，
(H1 T)v＝Mz(α1)My(θ1)Mz(-α1)，(v＝1,2,…,q-1),
Figure GDA0002524105770000083
Figure GDA0002524105770000084
Figure GDA0002524105770000085
Mz(α1)为第一个软体驱动器的第v个特征点的位置矢量绕z轴转动α1的旋转矩阵，
My(θ1)为第一个软体驱动器的第v个特征点的位置矢量绕y轴转动θ1的旋转矩阵，
Mz(-α1)为第一个软体驱动器的第v个特征点的位置矢量绕z轴转动-α1的旋转矩阵，
(H1 T)v为第一个软体驱动器的第v个特征点在第一个软体驱动器底端坐标系的位置矢量的旋转矩阵；
采用上述方式算出第一个软体驱动器的前(i-1)个特征点在第一个软体驱动器底端坐标系的位置矢量的旋转矩阵之和[(H1 T)i-2+(H1 T)i-3+…+(H1 T)1+(H1 T)0]以及第一个软体驱动器的第(i-1)个特征点在第一个软体驱动器底端坐标系的位置矢量的旋转矩阵(H1 T)i-1，从而确定第一个软体驱动器上第i个特征点在第一个软体驱动器的底端的坐标系中的位置矢量r1,i。
将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中，包括以下步骤：当软体驱动器排第一时，则无需转换；当软体驱动器排第二至M时，则将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到前一个软体驱动器的底端的坐标系中，依次类推，将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中。
特征点在固定底端坐标系中的空间姿态采用固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角表示。获得特征点在固定底端坐标系中的空间姿态，包括以下步骤：确定特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系；根据特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系，确定固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角。固定底端坐标系依次绕z轴、y轴、x轴旋转至与特征点所在的软体驱动器底端的坐标系重合。
当确定的特征点为第q个软体驱动器上第i个特征点，确定第q个软体驱动器上第i个特征点在第q个软体驱动器底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系，包括以下步骤：第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标为(xq,i yq,i zq,i)T，第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标为(x0 y0 z0)T，第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系为：
Figure GDA0002524105770000101
Figure GDA0002524105770000103
Figure GDA0002524105770000102
其中，
(Hu T)v＝Mz(αu)My(θu)Mz(-αu)，(u＝1,2,…,M)，(v＝1,2,…,Nu)，
(Hu T)v为第u个软体驱动器上第v个特征点在第u个软体驱动器底端的坐标系中位置矢量的旋转矩阵，
αu为第u个软体驱动器的第v个特征点处的弯曲方向，θu为第u个软体驱动器的第v个特征点处的弯曲角度，
Mz(αu)为第u个软体驱动器的第v个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转αu的旋转矩阵，
My(θu)为第u个软体驱动器的第v个特征点在第u个软体驱动器底端坐标系中的位置矢量绕y轴旋转θu的旋转矩阵，
Mz(-αu)为第u个软体驱动器的第v个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转-αu的旋转矩阵，
采用上述方式计算出前(q-1)个软体驱动器上末端的特征点在所在的软体驱动器底端的坐标系中位置矢量的旋转矩阵的乘积
Figure GDA0002524105770000104
以及第q个软体驱动器上第(i-1)个特征点在第q个软体驱动器底端的坐标系中位置矢量的旋转矩阵[(Hq T)i-1]，从而确定固定底端坐标系按照3-2-1顺序分别旋转欧拉角α0、β0、γ0后与第q个软体驱动器的底端的坐标系重合的旋转矩阵[Rz(α0)Ry(β0)Rx(γ0)]，进而确定旋转欧拉角α0、β0、γ0。
本实施例的软体机械臂空间运动姿态的开环控制方法，包括以下步骤：采用上述软体机械臂的力学模型对软体机械臂的指定运动姿态进行模拟，从而确定软体机械臂的各个气囊体的气压值；分别将确定的气压值对应输入软体机械臂的各个气囊体中，使软体机械臂形成指定运动姿态。
本实施例的软体机械臂空间运动姿态的闭环控制方法，包括以下步骤：分别向软体机械臂的各个气囊体输入初始的压力值，使软体机械臂形成运动姿态；采用上述软体机械臂的力学模型确定软体机械臂的运动姿态的姿态信息；根据软体机械臂的运动姿态的姿态信息与指定运动姿态的姿态信息之间的差异对输入的压力值进行调整，直至软体机械臂形成的运动姿态为指定运动姿态。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种软体机械臂的力学模型的建立方法，用于获取软体机械臂上空间位置和空间姿态以对软体机械臂的运动进行控制，
软体机械臂包括M个软体驱动器(1)，软体驱动器(1)通过连接结构(2)沿轴向首尾密封连接，
软体驱动器(1)包括沿周向等距排布的多个波纹管状的气囊体(11)，每个气囊体(11)包括多个波纹小节，波纹小节的分布中心为特征点，
M个软体驱动器(1)从软体机械臂的固定端向自由端方向依次排列为第一个软体驱动器，第二个软体驱动器，……，第M个软体驱动器；M个软体驱动器(1)上波纹小节的个数依次为N1，N2，……，NM；M个软体驱动器(1)上特征点的个数依次为N1，N2，……，NM，
其特征在于，建立方法包括以下步骤：
软体驱动器上靠近软体机械臂固定端的一端为底端，在每个软体驱动器的底端建立B-xyz三维坐标系，位于软体机械臂固定端处的第一个软体驱动器的坐标系为固定底端坐标系；
从M个软体驱动器中任取一个作为第q个软体驱动器，从第q个软体驱动器的底端依次确定第i(1≤i≤Nq)个特征点在第q个软体驱动器底端的坐标系中的弯曲方向、弯曲角度以及位置坐标，建立第q个软体驱动器的力学模型，以相同方式分别建立M个软体驱动器的力学模型；
将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中；
根据M个软体驱动器的力学模型、特征点在所在的软体驱动器底端的坐标系中的位置坐标以及特征点在固定底端坐标系中的位置坐标，确定输入气囊体的压力值与软体机械臂上各个特征点的空间位置和姿态之间的关系，建立软体机械臂的力学模型；
特征点在固定底端坐标系中的空间姿态采用固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角表示；
获得特征点在固定底端坐标系中的空间姿态，包括以下步骤：
确定特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系；
根据特征点在所在的软体驱动器底端的坐标系中的位置坐标与特征点在固定底端坐标系中的位置坐标之间的关系，确定固定底端坐标系到特征点所在的软体驱动器底端的坐标系的3-2-1顺序旋转的欧拉角；
当确定的特征点为第q个软体驱动器上第i个特征点，确定第q个软体驱动器上第i个特征点在第q个软体驱动器底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系，
包括以下步骤：
第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标为(xq,i yq,i zq,i)T，第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标为(x0y0 z0)T，
第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标与第q个软体驱动器上第i个特征点在固定底端坐标系中的位置坐标之间的关系为：
Figure FDA0002605301340000021
，
Hu(αu,θu)＝Mz(αu)My(θu)Mz(-αu)，u＝1，2，……，(q-1)，
Figure FDA0002605301340000022
Figure FDA0002605301340000023
其中，
Figure FDA0002605301340000024
为前(q-1)个软体驱动器上末端的特征点在所在的软体驱动器底端的坐标系中位置矢量的旋转矩阵的乘积，
Figure FDA0002605301340000025
为第q个软体驱动器上第(i-1)个特征点在第q个软体驱动器底端的坐标系中位置矢量的旋转矩阵，
Figure FDA0002605301340000026
中每个Hu(u＝1,2,...,q)矩阵的定义表达式，αu,θu是Hu矩阵的自变量，αu为第u个软体驱动器的弯曲方向，θu为第u个软体驱动器的弯曲角度，
Mz(αu)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转αu的旋转矩阵，
My(θu)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕y轴旋转θu的旋转矩阵，
Mz(-αu)为第u个软体驱动器的第Nu个特征点在第u个软体驱动器底端坐标系中的位置矢量绕z轴旋转-αu的旋转矩阵，
Rz(α0)Ry(β0)Rx(γ0)为固定底端坐标系按照3-2-1顺序分别旋转欧拉角α0、β0、γ0后与第q个软体驱动器的底端的坐标系重合的旋转矩阵。
2.根据权利要求1所述的软体机械臂的力学模型的建立方法，其特征在于，软体驱动器的底端建立的坐标系，以软体驱动器中任意一个气囊体位于底端的波纹小节的中心点为坐标原点B。
3.根据权利要求1所述的软体机械臂的力学模型的建立方法，其特征在于，确定第q个软体驱动器的第i个特征点在第q个软体驱动器的底端的坐标系中的弯曲方向和弯曲角度，包括以下步骤：
分别根据第q个软体驱动器中波纹小节的弹性系数、气囊体内腔的横截面积、波纹小节的原始长度以及不同气囊体中的气压值，分别确定第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度；
根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度，确定第q个软体驱动器的第i个特征点处的弯曲方向；
根据第q个软体驱动器的不同气囊体的第i个波纹小节与气囊体底端之间的长度、气囊体壁体的横截面积、波纹小节的原始长度、气囊体之间的间距、气囊体的惯性距以及波纹小节的弹性系数，确定第q个软体驱动器的第i个特征点处的弯曲角度。
4.根据权利要求3所述的软体机械臂的力学模型的建立方法，其特征在于，确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标，包括以下步骤：
根据第q个软体驱动器的第i个特征点的弯曲方向、波纹小节的原始长度、波纹小节的弹性系数以及气囊体壁体的横截面积，分别计算第q个软体驱动器的不同气囊体的第i个波纹小节的中心点在第q个软体驱动器的底端的坐标系中的位置矢量；
根据第q个软体驱动器的不同气囊体的第i个波纹小节的中心点的位置矢量以及第q个软体驱动器的前(i-1)个特征点的位置矢量在第q个软体驱动器的底端的坐标系中旋转矩阵，确定第q个软体驱动器上第i个特征点在第q个软体驱动器的底端的坐标系中的位置坐标。
5.根据权利要求1所述的软体机械臂的力学模型的建立方法，其特征在于，将M个软体驱动器上特征点的位置坐标均转换到固定底端坐标系中，包括以下步骤：
当软体驱动器排第一时，则无需转换；
当软体驱动器排第二至M时，则将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到前一个软体驱动器的底端的坐标系中，依次类推，将特征点在所在的软体驱动器底端的坐标系中的位置坐标转换到固定底端坐标系中。
6.一种软体机械臂空间运动姿态的控制方法，其特征在于，包括以下步骤：
采用权利要求1-5任一项所述的软体机械臂的力学模型对软体机械臂的指定的空间运动姿态进行模拟，从而确定输入软体机械臂的各个气囊体中的气压值；
分别将确定的气压值对应输入软体机械臂的各个气囊体中，使软体机械臂形成指定的空间运动姿态。
7.一种软体机械臂空间运动姿态的控制方法，其特征在于，包括以下步骤：
分别向软体机械臂的各个气囊体输入初始的压力值，使软体机械臂在空间运动；
采用权利要求1-5任一项所述的软体机械臂的力学模型确定软体机械臂的空间位置和空间姿态；
根据软体机械臂的空间位置和空间姿态与指定的空间运动姿态之间的差异对输入气囊体中的压力值进行调整，直至软体机械臂形成的空间运动姿态为指定的空间运动姿态。
CN201910386781.5A 2019-05-10 2019-05-10 软体机械臂的力学模型的建立方法及控制方法 Active CN110125933B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910386781.5A CN110125933B (zh) 2019-05-10 2019-05-10 软体机械臂的力学模型的建立方法及控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910386781.5A CN110125933B (zh) 2019-05-10 2019-05-10 软体机械臂的力学模型的建立方法及控制方法

Publications (2)

Publication Number Publication Date
CN110125933A CN110125933A (zh) 2019-08-16
CN110125933B true CN110125933B (zh) 2020-10-13

Family

ID=67577044

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910386781.5A Active CN110125933B (zh) 2019-05-10 2019-05-10 软体机械臂的力学模型的建立方法及控制方法

Country Status (1)

Country Link
CN (1) CN110125933B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110744548B (zh) * 2019-11-08 2021-02-19 山东大学 一种多线驱连续体机械臂驱动线耦合关系的统一解耦方法
CN111546339B (zh) * 2020-05-11 2021-01-05 中国人民解放军军事科学院国防科技创新研究院 一种气囊型软体机械臂的闭环运动控制方法及系统
CN111546341B (zh) * 2020-05-11 2020-12-18 中国人民解放军军事科学院国防科技创新研究院 气囊型软体机械臂的开环运动控制方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103978485A (zh) * 2014-05-15 2014-08-13 郑天江 一种仿蛇型软体机器人控制系统和控制方法
CN106055794A (zh) * 2016-05-31 2016-10-26 浙江科澜信息技术有限公司 一种参数化建立三维管线模型的方法
CN107263477A (zh) * 2017-07-07 2017-10-20 浙江大学 一种绳驱动串联关节型蛇形机械臂控制方法
CN108908331A (zh) * 2018-07-13 2018-11-30 哈尔滨工业大学（深圳） 超冗余柔性机器人的避障方法及系统、计算机存储介质
CN108943010A (zh) * 2018-06-29 2018-12-07 中国人民解放军国防科技大学 气控型刚柔耦合模块化软体机械臂

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101804675B1 (ko) * 2016-05-18 2017-12-04 서울대학교산학협력단 소프트 로봇 구동기 유닛 및 이를 구비한 소프트 로봇
CN106078742B (zh) * 2016-06-29 2018-04-24 北京科技大学 一种针对带有输出约束的柔性机械臂的振动控制方法
CN109664312A (zh) * 2019-01-18 2019-04-23 哈尔滨工业大学 基于热变形的生长型软体机器人预设变形方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103978485A (zh) * 2014-05-15 2014-08-13 郑天江 一种仿蛇型软体机器人控制系统和控制方法
CN106055794A (zh) * 2016-05-31 2016-10-26 浙江科澜信息技术有限公司 一种参数化建立三维管线模型的方法
CN107263477A (zh) * 2017-07-07 2017-10-20 浙江大学 一种绳驱动串联关节型蛇形机械臂控制方法
CN108943010A (zh) * 2018-06-29 2018-12-07 中国人民解放军国防科技大学 气控型刚柔耦合模块化软体机械臂
CN108908331A (zh) * 2018-07-13 2018-11-30 哈尔滨工业大学（深圳） 超冗余柔性机器人的避障方法及系统、计算机存储介质

Also Published As

Publication number Publication date
CN110125933A (zh) 2019-08-16

Similar Documents

Publication Publication Date Title
CN110125933B (zh) 软体机械臂的力学模型的建立方法及控制方法
CN110270987B (zh) 气驱动软体爬行机器人及其制造和控制方法
Chen et al. Soft origami gripper with variable effective length
CN110744547B (zh) 一种基于分段常曲率的连续体机械臂逆运动学建模方法
CN110253574B (zh) 一种多任务机械臂位姿检测和误差补偿方法
CN109702751A (zh) 一种七自由度串联机械臂的位置级逆解方法
CN106844951B (zh) 基于分段几何法求解超冗余机器人逆运动学的方法及系统
Zheng et al. A 3D dynamic model for continuum robots inspired by an octopus arm
CN109176487A (zh) 一种联动关节段标定方法、系统、设备、存储介质
CN110125934B (zh) 软体机械臂的运动学模型的校正方法
CN113334381A (zh) 一种可运动解耦的连续体机器人控制方法
Kobayashi et al. Active-bending six-bar tensegrity modular robot driven by thin artificial muscles
CN113084829A (zh) 一种基于可精准控制骨架的仿生软体消毒机械臂
Ma Design and modelling of a continuum robot with soft stiffness-adjustable elements for confined environments
US20220305668A1 (en) Soft joint gripper based on 4d printing and consistency control method thereof
Mishra et al. Dynamic modelling of an elephant trunk like flexible bionic manipulator
Zou et al. Design and implement of shape detection for the soft manipulator
Li et al. Model and control of hybrid hard-soft robots using model predictive control
Pachouri et al. Design and modeling of a planar-to-spatial tendon-driven continuum manipulator subjected to uncertain forces
CN109927041B (zh) 运动关节块、可表达情感的机器人及其控制方法
Yang et al. Monitoring and control of position and attitude of flexible manipulator with three degrees of freedom
CN112356020B (zh) 一种变截面柔性臂反馈控制的线长定位方法、装置和介质
Singh et al. Ph model-based shape reconstruction of heterogeneous continuum closed loop kinematic chain: An application to skipping rope
JP4647919B2 (ja) 制御方法および制御装置
Abdulrab et al. Snake-like soft robot using 2-chambers actuator

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