CN116238616A - 一种液压驱动并联结构的控制方法与装置 - Google Patents

Abstract

本发明公开了一种液压驱动并联结构的控制方法及装置，获取机器人足部的俯仰角位移量和滚转角位移量；根据俯仰角位移量和滚转角位移量构建机器人的足部旋转矩阵；基于足部旋转矩阵计算液压缸在转动后两端部的坐标值；基于每个液压缸在转动后两端部的坐标值计算转动后液压缸的第一长度，并根据第一长度和液压缸初始长度计算液压缸的伸长量，得到伸长量与角位移量的第一关系式；本发明通过建立足心坐标系，并将踝关节各个关键点的坐标值和足部的俯仰角位移量与滚转角位移量相结合，以计算得到旋转轴踝关节液压缸的长度，最终通过液压缸长度变化量对液压缸进行调节，可以解耦机器人踝关节的俯仰和滚转两自由度运动能力。

Description

一种液压驱动并联结构的控制方法与装置

技术领域

本发明属于液压仿人双足机器人控制技术领域，尤其涉及一种液压驱动并联结构的控制方法与装置。

背景技术

仿人足式机器人的双足踝关节处于整个机器人结构的下肢末端位置，为了实现机器人运动中踝关节的类人设计，就需要具备基本的两个自由度：俯仰自由度和滚转自由度。

目前大多数机器人踝关节的俯仰和滚转两自由度采用串联机构设计，这样可以很便捷地实现两个自由度的独立控制。串联机构在每个自由度配置一个驱动器，这种设计方法的两自由度控制本身就是独立的，不需要解耦算法即可实现两个自由度的独立控制，因此串联结构的分析十分简单。

目前为了实现仿人的踝关节设计，也有踝关节两自由度的并联结构设计，踝关节的俯仰和滚转两自由度由两个液压缸共同驱动。但是，由于缺乏两自由度的解耦方法，通常只考虑了踝关节的俯仰自由度的主动运动，没有考虑踝关节两自由度的协同动作控制方法。

发明内容

本发明的目的是提供一种液压驱动并联结构的控制方法与装置，以实现仿人踝关节的俯仰和滚转两自由度解耦控制。

本发明采用以下技术方案：一种液压驱动并联结构的控制方法，包括以下步骤：

获取机器人足部的俯仰角位移量和滚转角位移量；

根据俯仰角位移量和滚转角位移量构建机器人的足部旋转矩阵；

基于足部旋转矩阵计算液压缸在转动后两端部的坐标值；其中，坐标值为足心坐标系中的坐标值，足心坐标系的原点为踝关节的中心，X轴朝向足部前方，Y轴朝向足部侧方，Z轴朝向足部上方；

基于每个液压缸在转动后两端部的坐标值计算转动后液压缸的第一长度，并根据第一长度和液压缸初始长度计算液压缸的伸长量，得到伸长量与角位移量的第一关系式；其中，角位移量包括俯仰角位移量和滚转角位移量。

进一步地，利用插值法对关系式进行拟合，得到伸长量与角位移量的第二关系式。

进一步地，还包括：

根据第二关系式确定液压缸对踝关节旋转中心的力矩；

根据液压缸对踝关节旋转中心的力矩，确定踝关节的俯仰自由度力矩和滚转自由度力矩。

进一步地，确定踝关节的俯仰自由度力矩和滚转自由度力矩包括：

将每个液压缸对踝关节旋转中心的力矩分解为X轴力矩和Y轴力矩；

融合两个液压缸对踝关节旋转中心的X轴力矩，生成踝关节滚转自由度力矩；

融合两个液压缸对踝关节旋转中心的Y轴力矩，生成踝关节俯仰自由度力矩。

进一步地，足部旋转矩阵为：

其中，R为足部旋转矩阵，C_p＝cosθ_p，S_p＝sinθ_p，S_r＝sinθ_r，C_r＝cosθ_r，θ_p为俯仰角位移量，θ_r为滚转角位移量。

进一步地，第一关系式为：

其中，ΔL₁为踝关节中第一个液压缸的伸长量，L₀为第一个液压缸的初始长度，α₁＝-aC_p+cS_pS_r-dS_pC_r+b，a为液压缸下端十字轴中心点和踝关节中心点连线在X轴上的投影长度，b为液压缸上端十字轴中心点和踝关节中心点连线在X轴上的投影长度，c为液压缸下端十字轴中心点和踝关节中心点连线在Y轴上的投影长度，d为液压缸下端十字轴中心点和踝关节中心点连线在Z轴上的投影长度，β₁＝cC_r+dS_r-c，γ₁＝aS_p+cC_pS_r-dC_pC_r-h，h为液压缸上端十字轴中心点和踝关节中心点连线在Z轴上的投影长度，ΔL₂为踝关节中第二个液压缸的伸长量，α₂＝-aC_p-cS_pS_r-dS_pC_r+b，β₂＝-cC_r+dS_r+c，γ₂＝aS_p-cC_pS_r-dC_pC_r-h。

进一步地，第二关系式为：

其中，p₀₀、p₁₀、p₀₁、p₂₀、p₁₁、p₀₂、p₃₀、p₂₁、p₁₂、p₀₃、p₄₀、p₃₁、p₂₂、p₁₃、p₀₄、p′₀₀、p′₁₀、p′₀₁、p′₂₀、p′₁₁、p′₀₂、p′₃₀、p′₂₁、p′₁₂、p′₀₃、p′₄₀、p′₃₁、p′₂₂、p′₁₃和p′₀₄均为常数。

进一步地，液压缸对踝关节旋转中心的力矩为：

其中，

为第一个液压缸对踝关节旋转中心的力矩，f₁为力的大小系数，L₁为液压缸运动后的长度，i、j、k为三个互相垂直的单位向量。

本发明的另一种技术方案：一种液压驱动并联结构的控制装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述的一种液压驱动并联结构的控制方法。

本发明的有益效果是：本发明通过建立足心坐标系，并将踝关节各个关键点的坐标值和足部的俯仰角位移量与滚转角位移量相结合，以计算得到旋转轴踝关节液压缸的长度，最终通过液压缸长度变化量对液压缸进行调节，可以解耦机器人踝关节的俯仰和滚转两自由度运动能力。

附图说明

图1为本发明实施例中的踝关节几何简化模型示意图；

图2为本发明实施例中踝关节结构简化图；

图3为本发明实施例中踝关节两个液压缸的伸长量随运动滚转角和俯仰角的变化曲线图；

图4为本发明实施例中运动学解耦后踝关节两自由度角度跟踪示意图；

图5为本发明实施例中动力学解耦后踝关节两自由度角度跟踪示意图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明可应用于液压驱动的仿人双足机器人的踝关节控制领域。对于仿人双足机器人，一方面机器人下肢末端位置结构空间有限，另一方面俯仰自由度和滚转自由度这两个独立的自由度需要依靠两个并联安装液压缸来起到实现自由度独立控制和提供踝关节动作所需驱动力的作用。

本发明设计的机器人仿人踝关节中，如图1所示，采用以十字轴为关节转动中心，将十字轴安装在足部上方中心位置，以直线型液压驱动缸为驱动器的并联机构设计，即在足部后方安装两个竖向设置的液压缸，且两个液压缸在左右方向上并联设置，两个液压缸输出轴的底端和足部后方连接、顶端和机器人小腿部转动连接。因为并联机构的特点是所有分支机构可以同时接受驱动器输入，然后共同决定执行机构的输出，所以并联机构间的运动具有耦合关系，其计算分析相比于串联机构更为复杂。

要实现对踝关节的柔顺性控制，首先要实现对其的力伺服控制。对机器人进行力控制，明确液压系统的输入量是必不可少的明确液压缸的控制信号，就需要建立踝关节位姿和液压缸状态之间变化关系的数学模型。又因为踝关节结构上的两个并联液压缸独立控制，所以，对机器人踝关节进行柔顺性控制的前提是解决踝关节俯仰和滚转的运动角度变化与液压缸长度变化和液压缸输出力变化之间的一一映射，即解决踝关节的运动学和动力学解耦问题。

更为具体的，本发明所用液压双足机器人的踝关节驱动装置采用的是两个完全相同的并列安装的液压缸，连接件均选用可实现两自由度转动的十字轴，液压缸的上下两端分别通过十字轴与小腿支撑耳和足部支撑耳相连。通过液压缸的伸缩可以实现踝关节绕连接足部和小腿结构的十字轴转动的俯仰运动和滚转运动。踝关节的几何简化模型如图1所示，踝关节的结构参数参见表1。

表1

具体的，本发明公开了一种液压驱动并联结构的控制方法，包括以下步骤：获取机器人足部的俯仰角位移量和滚转角位移量；根据俯仰角位移量和滚转角位移量构建机器人的足部旋转矩阵；基于足部旋转矩阵计算液压缸在转动后两端部的坐标值；其中，坐标值为足心坐标系中的坐标值，足心坐标系的原点为踝关节的中心，X轴朝向足部前方，Y轴朝向足部侧方，Z轴朝向足部上方；基于每个液压缸在转动后两端部的坐标值计算转动后液压缸的第一长度，并根据第一长度和液压缸初始长度计算液压缸的伸长量，得到伸长量与角位移量的第一关系式；其中，角位移量包括俯仰角位移量和滚转角位移量。

本发明通过建立足心坐标系，并将踝关节各个关键点的坐标值和足部的俯仰角位移量与滚转角位移量相结合，以计算得到旋转轴踝关节液压缸的长度，最终通过液压缸长度变化量对液压缸进行调节，可以解耦机器人踝关节的俯仰和滚转两自由度运动能力。

首先，定义θ_p是机器人足部在俯仰自由度(pitch)上的角位移量，θ_r是机器人足部滚转自由度(roll)上的角位移量。接着，对踝关节的几何模型建立足心坐标系和参考坐标系，如图1所示，图中建系方法如下：

坐标系{0}系为足心坐标系，是机器人足部初始位置所形成的初始坐标系，即足部发生运动前形成的初始坐标系，该系可看作一个固联在小腿结构上的坐标系。因为整个踝关节和足部的运动可看作是足部相对小腿的运动，当单独对踝关节的运动进行分析时，可看作其相对小腿是静止的，即相对大地坐标系静止，此时坐标系{0}系可看作与大地坐标系{W}系固连的坐标系。

坐标系{1}系为参考坐标系，是与足部固联的动坐标系，即踝关节初始坐标系{0}系绕轴旋转θ_p、θ_r后形成的新坐标系。将坐标系{0}系分别绕其X轴和Y轴旋转θ_p和θ_r角度后得到坐标系{1}系，通过齐次坐标的旋转变换关系可以得到{0}系和{1}系两坐标系之间的变换关系，旋转矩阵如下两式所示：

按照刚体旋转公式可以计算得出从坐标系{0}系到坐标系{1}系的齐次坐标旋转变换矩阵R为：

为简化书写，将C_p＝cosθ_p，S_p＝sinθ_p，S_r＝sinθ_r，C_r＝cosθ_r，，则可以得到旋转矩阵R的简化表达为：

其中，R为足部旋转矩阵。

由于本发明液压双足机器人是对称结构，左右双足结构对称，因此，左右两足的解耦计算方法相同，只是建系方式和相关坐标略有不同。下面以双足机器人的左脚为例，对踝关节的运动学解耦和动力学解耦进行分析。

本发明的柔顺性控制方法以机器人的踝关节作为研究对象，对踝关节液压驱动杠缸的并联机构运动学分析包含了对其的位置分析，速度加速度分析和工作空间分析等内容。位置分析是最基础的分析，主要分析机构的输入输出间的位置关系，即液压缸伸缩量和关节的运动角度之间的关系。本本发明所用机器人踝关节结构的运动有俯仰和滚转两个自由度的运动，足部在两个自由度上有θ_p和θ_r的偏转时，踝关节结构简化图的左视图和正视图如图2所示。

如图2所示，A₁、A₂是两液压缸下端与足部支撑耳相连的十字轴中心，它们在{0}系中的坐标表示为：

⁰A₁＝[-a,c,-d]^T (5)

⁰A₂＝[-a,-c,-d]^T (6)

B₁、B₂是两液压缸上端与小腿支撑耳相连的十字轴中心，它们在{0}系中的坐标表示为：

⁰B₁＝[-b,c,h]^T (7)

⁰B₂＝[-b,-c,h]^T (8)

可知，{0}系和{1}系两坐标系下的坐标变换关系：

¹A_i＝R*⁰A_i (9)

其中，i＝1或2。

进而，根据公式(5)、(6)和(9)可以计算得出A₁、A₂旋转后在{0}系中的坐标表示为：

由公式(7)、(8)、(10)和(11)得到：

因此，液压缸的运动后的长度L₁,L₂可由几何关系表示为：

由公式(12)、(13)、(14)计算得出坐标系{0}系下两个液压缸在踝关节运动后的实际长度L₁,L₂为：

从而，由公式(12)、(13)、(14)计算得出坐标系{0}系下两个液压缸在踝关节运动后的实际长度L₁,L₂为：

期望的解耦后两个驱动液压缸长度和足部俯仰角度和滚转角度之间的非线性关系如下所示：

[L₁ L₂]＝f[θ₁ θ₂] (18)

从而得到了{0}系下两个液压缸的伸长量ΔL₁,ΔL₂与踝关节俯仰角和滚转角运动角度间的关系分别为：

公式(19)和(20)即本发明的第一关系式，其中，ΔL₁为踝关节中第一个液压缸的伸长量，L₀为第一个液压缸的初始长度，α₁＝-aC_p+cS_pS_r-dS_pC_r+b，a为液压缸下端十字轴中心点和踝关节中心点连线在X轴上的投影长度，b为液压缸上端十字轴中心点和踝关节中心点连线在X轴上的投影长度，c为液压缸下端十字轴中心点和踝关节中心点连线在Y轴上的投影长度，d为液压缸下端十字轴中心点和踝关节中心点连线在Z轴上的投影长度，β₁＝cC_r+dS_r-c，γ₁＝aS_p+cC_pS_r-dC_pC_r-h，h为液压缸上端十字轴中心点和踝关节中心点连线在Z轴上的投影长度，ΔL₂为踝关节中第二个液压缸的伸长量，α₂＝-aC_p-cS_pS_r-dS_pC_r+b，β₂＝-cC_r+dS_r+c，γ₂＝aS_p-cC_pS_r-dC_pC_r-h。

由表达式(19)和(20)可知，两个液压缸驱动踝关节运动时的伸长量的表达式中含有大量的三角函数，具有很强的非线性特性，根据踝关节实际运动中的所需的角度变化裕度，本发明只选取

的角度变化范围进行分析，从而得到了坐标系{0}系下左足左右两个液压缸的伸长量随踝关节运动滚转角和俯仰角的变化曲线如图3所示，其中，图3(a)为左侧液压缸伸长量，图3(b)为右侧液压缸伸长量。

如上，完成了数值上对踝关节的运动学解耦，实现了两个并联液压缸长度变化和踝关节俯仰和滚转两个自由度运动角度之间关系的运动学解耦。将机器人踝关节规划的俯仰角度和滚转角度作为输入，通过解算后转化为两个独立液压缸的长度信号对液压驱动缸进行控制；液压缸长度变化后驱动踝关节角度发生变化，产生当前踝关节两自由度角度信号；将当前的角度信号解算后同样转化为液压缸长度变化量，从而形成控制闭环对机器人进行控制。

在机器人运动的虚拟物理模型中实现运动学解耦后踝关节俯仰、滚转两自由度角度跟踪曲线如图4所示，由图中可以看出，通过设计的踝关节两自由度运动学解耦方法，液压双足机器人踝关节俯仰角和滚转角的两自由度角度变化可以通过两个并联的独立液压缸的长度变化完美实现。

需要指出的是，对机器人踝关节的运动学解耦方法进行了详细的分析，实现了精准的数值解算。但是，在真实的机器人控制中需要将规划的关节角度转换到液压缸的伸长量，L₁和L₂的表达式包含了大量的三角函数运算，表达式复杂、运算量大，会占用大量运算时间，进而影响机器人控制的实时性，可以通过对函数用插值方法进行拟合来解决计算效率的问题。

本发明实施例中，利用插值法对关系式进行拟合，得到伸长量与角位移量的第二关系式。在对非线性表达式进行拟合时，找到一个合理的精度范围，将含有大量三角函数的非线性表达转化为比较简洁的形式，就可以在保证计算结果精度不受影响的前提下，提高运算效率，减小延迟。

具体的地，第二关系式为：

/>

其中，p₀₀、p₁₀、p₀₁、p₂₀、p₁₁、p₀₂、p₃₀、p₂₁、p₁₂、p₀₃、p₄₀、p₃₁、p₂₂、p₁₃、p₀₄、p′₀₀、p′₁₀、p′₀₁、p′₂₀、p′₁₁、p′₀₂、p′₃₀、p′₂₁、p′₁₂、p′₀₃、p′₄₀、p′₃₁、p′₂₂、p′₁₃和p′₀₄均为常数。拟合后的常数表示为：

在控制中用拟合后的线性表达式式(21)和式(22)替换含有大量非线性表达式的式(19)和式(20)，即可有效提高运算速度，且不影响所需运算精度，实现了提高机器人控制算法运算效率的目标。

在另一个实施例中，进行上述步骤后还包括：根据第二关系式确定液压缸对踝关节旋转中心的力矩；根据液压缸对踝关节旋转中心的力矩，确定踝关节的俯仰自由度力矩和滚转自由度力矩。

更具体的，确定踝关节的俯仰自由度力矩和滚转自由度力矩包括：将每个液压缸对踝关节旋转中心的力矩分解为X轴力矩和Y轴力矩；融合两个液压缸对踝关节旋转中心的X轴力矩，生成踝关节滚转自由度力矩；融合两个液压缸对踝关节旋转中心的Y轴力矩，生成踝关节俯仰自由度力矩。

由理论力学可知空间力对点的矩可以表示为力矩矢量

其中，

为作用力，/>

为力矩点到作用力上一点的矢量。

取

对左侧液压缸进行计算分析可得：

其中，

为沿液压缸方向的液压缸输出力，f₁为力的大小系数，L₁为液压缸运动后的长度，/>

进而根据公式(21)可以得到左侧液压缸对踝关节旋转中心的力矩τ₁为：

其中，

为第一个液压缸对踝关节旋转中心的力矩，i、j、k为三个互相垂直的单位向量。取X轴方向向量/>

Y轴方向向量/>

机器人踝关节的滚转运动可看作绕X轴的运动，俯仰运动可看作绕Y轴的运动。因此，将液压缸对踝关节旋转中心点的矩分解到X轴和Y轴上即可分别得到液压缸输出力对滚转和俯仰两个自由度运动角度的力矩。

将左侧液压缸对踝关节旋转中心的力矩τ₁分解到X轴和Y轴上可以得到：

对右侧液压缸旋转力矩的分析原理与左侧液压缸相同，此处不再赘述，同理可得右侧液压缸的力矩如下：

由此计算得到机器人左足踝关节滚转自由度的力矩τ_r和俯仰自由度的力矩τ_p如下：

综上，完成了数值上对踝关节的动力学解耦，实现了对液压缸输出力和机器人踝关节俯仰和滚转两个自由度运动角度变化间的关系解算。以左脚踝关节为例，动力学解耦实现后踝关节俯仰、滚转两自由度角度跟踪曲线如图5所示，由图中可以得出，通过设计的踝关节两自由度动力学解耦算法后，液压双足机器人踝关节俯仰和滚转两自由度的角度变化可以通过控制两个并联的独立液压缸的输出力完美实现。

综上所述，按照控制参考输入信号分类，对液压双足机器人的运动控制可分为位置控制和力控制两种。其中，位置控制有很大的局限性，在位置精度要求较高或者环境刚度较大的情况下，机器人末端执行器与环境之间相互作用力很大的问题不容忽视，这不止会为机器人的控制稳定性带来很大的问题，还有可能会对机器人的本体机械结构和环境带来很大的不可逆损伤。同时，实现机器人踝关节的柔顺性控制需要先实现踝关节的力伺服控制。因此，实现对机器人的力伺服控制十分有必要。

本发明实现了对于所用并联结构设计踝关节装置的运动学控制和动力学控制的建模和分析，得到了运动学和动力学的解耦方法，实现了数值上的解算和物理模型中的控制实现，并在matlab中集成了控制模块。

具体的，本发明首先完成了机械结构上对仿人踝关节的并联结构的设计，设计了一种基于十字轴链接的、具有俯仰和滚转两自由度运动能力的仿人机器人踝关节结构。然后，针对该仿人机器人的踝关节结构，结合其并联机械结构设计特点，提出了一种液压驱动并联结构的仿人机器人踝关节两自由度运动学和动力学的解耦算法，实现了在解耦算法下踝关节俯仰运动和滚转运动在液压伺服驱动下的独立位置控制和力控制。同时，提出并验证了一种基于十字轴的液压驱动的踝关节并联结构解耦方法，包括运动学解耦和动力学解耦。实现了通过两个并联安装液压缸对踝关节在俯仰和偏转两个自由度上的独立控制。

本发明还公开了一种液压驱动并联结构的控制装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述的一种液压驱动并联结构的控制方法。

上述的装置可以是云端服务器等计算设备或可穿戴设备。该装置可包括但不仅限于处理器、存储器。本领域技术人员可以理解，该装置可以包括更多或更少的部件，或者组合某些部件，或者不同的部件，例如还可以包括输入输出设备、网络接入设备等。

处理器可以是中央处理单元(Central Processing Unit，CPU)，该处理器还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器在一些实施例中可以是所述装置的内部存储单元，例如装置的硬盘或内存。所述存储器在另一些实施例中也可以是所述装置的外部存储设备，例如所述装置上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器还可以既包括所述装置的内部存储单元也包括外部存储设备。所述存储器用于存储操作系统、应用程序、引导装载程序(BootLoader)、数据以及其他程序等，例如所述计算机程序的程序代码等。所述存储器还可以用于暂时地存储已经输出或者将要输出的数据。

需要说明的是，上述装置的具体内容，由于与本发明方法实施例基于同一构思，其具体功能及带来的技术效果，具体可参见方法实施例部分，此处不再赘述。

Claims (9)

1.一种液压驱动并联结构的控制方法，其特征在于，包括以下步骤：

获取机器人足部的俯仰角位移量和滚转角位移量；

根据所述俯仰角位移量和滚转角位移量构建机器人的足部旋转矩阵；

基于所述足部旋转矩阵计算液压缸在转动后两端部的坐标值；其中，所述坐标值为足心坐标系中的坐标值，所述足心坐标系的原点为踝关节的中心，X轴朝向足部前方，Y轴朝向足部侧方，Z轴朝向足部上方；

基于每个液压缸在转动后两端部的坐标值计算转动后液压缸的第一长度，并根据所述第一长度和液压缸初始长度计算所述液压缸的伸长量，得到所述伸长量与角位移量的第一关系式；其中，所述角位移量包括俯仰角位移量和滚转角位移量。

2.如权利要求1所述的一种液压驱动并联结构的控制方法，其特征在于，利用插值法对所述关系式进行拟合，得到所述伸长量与角位移量的第二关系式。

3.如权利要求2所述的一种液压驱动并联结构的控制方法，其特征在于，还包括：

根据所述第二关系式确定所述液压缸对踝关节旋转中心的力矩；

根据所述液压缸对踝关节旋转中心的力矩，确定踝关节的俯仰自由度力矩和滚转自由度力矩。

4.如权利要求3所述的一种液压驱动并联结构的控制方法，其特征在于，确定踝关节的俯仰自由度力矩和滚转自由度力矩包括：

将每个所述液压缸对踝关节旋转中心的力矩分解为X轴力矩和Y轴力矩；

融合两个液压缸对踝关节旋转中心的X轴力矩，生成踝关节滚转自由度力矩；

融合两个液压缸对踝关节旋转中心的Y轴力矩，生成踝关节俯仰自由度力矩。

5.如权利要求2-4任一所述的一种液压驱动并联结构的控制方法，其特征在于，所述足部旋转矩阵为：

其中，R为足部旋转矩阵，C_p＝cosθ_p，S_p＝sinθ_p，S_r＝sinθ_r，C_r＝cosθ_r，θ_p为俯仰角位移量，θ_r为滚转角位移量。

6.如权利要求5所述的一种液压驱动并联结构的控制方法，其特征在于，所述第一关系式为：

其中，ΔL₁为踝关节中第一个液压缸的伸长量，L₀为第一个液压缸的初始长度，α₁＝-aC_p+cS_pS_r-dS_pC_r+b，a为液压缸下端十字轴中心点和踝关节中心点连线在X轴上的投影长度，b为液压缸上端十字轴中心点和踝关节中心点连线在X轴上的投影长度，c为液压缸下端十字轴中心点和踝关节中心点连线在Y轴上的投影长度，d为液压缸下端十字轴中心点和踝关节中心点连线在Z轴上的投影长度，β₁＝cC_r+dS_r-c，γ₁＝aS_p+cC_pS_r-dC_pC_r-h，h为液压缸上端十字轴中心点和踝关节中心点连线在Z轴上的投影长度，ΔL₂为踝关节中第二个液压缸的伸长量，α₂＝-aC_p-cS_pS_r-dS_pC_r+b，β₂＝-cC_r+dS_r+c，γ₂＝aS_p-cC_pS_r-dC_pC_r-h。

7.如权利要求2所述的一种液压驱动并联结构的控制方法，其特征在于，所述第二关系式为：

其中，p₀₀、p₁₀、p₀₁、p₂₀、p₁₁、p₀₂、p₃₀、p₂₁、p₁₂、p₀₃、p₄₀、p₃₁、p₂₂、p₁₃、p₀₄、p′₀₀、p′₁₀、p′₀₁、p′₂₀、p′₁₁、p′₀₂、p′₃₀、p′₂₁、p′₁₂、p′₀₃、p′₄₀、p′₃₁、p′₂₂、p′₁₃和p′₀₄均为常数。

8.如权利要求6所述的一种液压驱动并联结构的控制方法，其特征在于，所述液压缸对踝关节旋转中心的力矩为：

其中，

为第一个液压缸对踝关节旋转中心的力矩，f₁为力的大小系数，L₁为所述液压缸运动后的长度，i、j、k为三个互相垂直的单位向量。

9.一种液压驱动并联结构的控制装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-8任一项所述的一种液压驱动并联结构的控制方法。

Images