CN117325152A - 一种阻抗控制方法 - Google Patents

Abstract

本申请公开了一种阻抗控制方法，包括：进行动力学建模，得到在关节空间下的封闭形式动力学方程；根据封闭形式动力学方程，得到驱动拉力的表达式；根据虚功原理，得到绳索速度向量、绳索拉力向量、关节速度向量、关节等效力矩的关系；通过关节转矩计算绳索拉力，驱动拉力解能够表示为最小二乘解与任意零空间解的和；对绳索拉力约束下的关节转矩输出能力进行分析；对绳驱关节的拉力传递过程建立理想数学模型，分析绳索空间拉力控制下的瞬态耦合效应；进行关节空间拉力控制下的关节运动分析；在关节空间下建立目标阻抗模型。本申请中，能有效地消除拉力控制过程的耦合现象，提高绳驱蛇形机械臂的主动柔顺性。

Description

一种阻抗控制方法

技术领域

本申请涉及柔顺控制技术领域，特别涉及一种阻抗控制方法。

背景技术

力柔顺控制可以根据其实现方式大致分为两类：基于位置内环控制，即导纳模型；基于力内环控制，即阻抗模型。

目前大多数研究着重于基于外力观测的导纳柔顺控制研究，而对于阻抗控制方法的讨论较少，尤其是针对多自由度的绳驱蛇形机械臂。在导纳模型下，通过已知外力，根据一定柔顺策略控制机械臂的位置，最直接的方法即由传感器直接测量外力。例如：从绳驱机械臂末端安装六维力传感器测量外部的作用力，通过导纳模型并由逆运动学得到期望关节位置。再由绳索力或绳索位混合控制组成的位置内环，实现较高精度的绳驱蛇形臂末端柔顺控制。但是这种方法需要安装尺寸较大且沉重的力传感器，对于大多数小尺寸、低负载绳驱蛇形臂并不适用。

发明内容

为解决上述技术问题中的至少之一，本申请提供一种阻抗控制方法，所采用的技术方案如下：

本申请提供一种阻抗控制方法，阻抗控制方法包括：

进行动力学建模，将递推形式动力学方程进行整理，得到在关节空间下的封闭形式动力学方程；

根据封闭形式动力学方程，得到驱动拉力的表达式；

根据虚功原理，结合关节到绳长的映射关系，得到绳索速度向量、绳索拉力向量、关节速度向量、关节等效力矩的关系；

通过关节转矩计算绳索拉力，驱动拉力解能够表示为最小二乘解与任意零空间解的和；

对绳索拉力约束下的关节转矩输出能力进行分析；

对绳驱关节的拉力传递过程建立理想数学模型，分析绳索空间拉力控制下的瞬态耦合效应；

进行关节空间拉力控制下的关节运动分析；

在关节空间下建立目标阻抗模型。

本申请的某些实施例中，所述进行动力学建模包括：

对末端连杆建立力平衡方程，结合D-H坐标系定义的关节转轴方向，得到万向关节的等效驱动转矩；

对其他连杆建立力平衡方程，结合D-H坐标系定义的关节转轴方向，得到万向关节的等效驱动转矩。

本申请的某些实施例中，所述得到驱动拉力的表达式包括：

通过绳索拉力分配算法找到可行的解；

根据绳索、关节空间映射矩阵得到驱动绳索拉力与等效关节驱动转矩的标量映射关系。

本申请的某些实施例中，所述驱动拉力解能够表示为最小二乘解与任意零空间解的和包括：

选取内拉力系数，内拉力位于绳索在关节段绳长函数的雅可比矩阵的右零空间，内拉力系数任意选取。

本申请的某些实施例中，所述选取内拉力系数包括：

求出最小绳索内拉力，使所有绳索拉力均大于拉力下限，得到拉力和的最小解；

求取使各驱动绳索拉力恰好等于拉力下限值的内拉力系数，并取其中的最大值；

使驱动绳索中最小拉力恰好等于拉力下限。

本申请的某些实施例中，所述对绳索拉力约束下的关节转矩输出能力进行分析包括：

建立坐标系，将输出转矩分解在正交的万向节转轴上；

由万向节转矩得到输出转矩方向；

由绳索拉力约束与输出转矩约束，能够构建优化函数，求取给定关节位置及给定输出转矩方向下的最大输出转矩。

本申请的某些实施例中，所述对绳索拉力约束下的关节转矩输出能力进行分析还包括：

假设各驱动绳索均以最大包络角穿行靠近基座端的关节绳孔，假设均采用静摩擦，得到最大拉力变化系数；

根据最大拉力变化系数，经过摩擦力作用后，得到实际驱动拉力。

本申请的某些实施例中，对绳驱关节的拉力传递过程建立理想数学模型，分析绳索空间拉力控制下的瞬态耦合效应包括：

忽略传动中摩擦，将传递部件的质量转换至滑块上，简化为滑块驱动绳索的模型；

对绳驱关节的模型作出假设；

根据拉力传递模型，得到驱动电机输出力与滑块运动的关系；

根据绳索刚度模型，通过滑块与关节角的位置差得到绳索拉力。

本申请的某些实施例中，对绳驱关节的拉力传递过程建立理想数学模型，分析绳索空间拉力控制下的瞬态耦合效应还包括：

对绳索拉力闭环控制中使用PD控制，得到期望电机电流；

得到关节运动与期望转矩输入的关系；

分析得出引起瞬态耦合现象的原因。

本申请的某些实施例中，所述进行关节空间拉力控制下的关节运动分析包括：

通过期望与等效实际输出转矩的比较，组成关节转矩的控制闭环；

通过关节、绳索空间转换得到电机的驱动电流信号，以削弱关节运动耦合效应。

本申请的实施例至少具有以下有益效果：本申请中，阻抗控制相较于导纳方案具有高动态响应、低抖动的优点；阻抗控制方法主要由动力学建模、绳索拉力分配与控制和关节阻抗控制方法三部分组成；为了简化建模难度，将刚性连杆所受作用力按照作用对象分为相邻连杆作用力、惯性力及绳索作用力；实际对绳驱臂进行控制的时候需要根据实时采集的运动参数进行控制输入的计算，以使绳驱臂的实际运动跟踪期望运动轨迹；对不同关节间产生的耦合运动进行分析，提出关节空间控制以消除耦合现象；对关节阻抗控制进行研究，以提高绳驱机械臂的柔顺性；阻抗控制方法能有效地消除拉力控制过程的耦合现象，提高绳驱蛇形机械臂的主动柔顺性。

本申请的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。

附图说明

本申请的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是本申请一个实施例提供的阻抗控制方法的流程图；

图2是本申请另一个实施例提供的阻抗控制方法的流程图；

图3是本申请另一个实施例提供的阻抗控制方法的流程图；

图4是本申请另一个实施例提供的阻抗控制方法的流程图；

图5是本申请另一个实施例提供的阻抗控制方法的流程图；

图6是本申请另一个实施例提供的阻抗控制方法的流程图；

图7是本申请另一个实施例提供的阻抗控制方法的流程图；

图8是本申请另一个实施例提供的阻抗控制方法的流程图；

图9是本申请另一个实施例提供的阻抗控制方法的流程图；

图10是本申请另一个实施例提供的阻抗控制方法的流程图；

图11是本申请中关节输出转矩方向示意图；

图12是本申请中绳驱关节拉力传递简化模型；

图13是本申请中绳索空间拉力控制流程图；

图14是本申请中关节空间拉力控制流程图；

图15是本申请中关节阻抗控制流程图。

具体实施方式

本部分将结合图1至图15详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本申请，而不能理解为对本申请的限制。

在本申请的描述中，需要理解的是，若出现术语“中心”、“中部”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。限定有“第一”、“第二”的特征是用于区分特征名称，而非具有特殊含义，此外，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本申请的描述中，除非另有说明，“多个”的含义是两个或两个以上。

在本申请的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。

本申请实施例提供一种阻抗控制方法，应用于绳驱机械臂，下面提出本申请的阻抗控制方法的各个实施例。

如图1所示，图1是本申请一个实施例提供的阻抗控制方法的流程图，该阻抗控制方法包括但不限于有步骤S110、步骤S120、步骤S130、步骤S140、步骤S150、步骤S160、步骤S170和步骤S180。

步骤S110，进行动力学建模，将递推形式动力学方程进行整理，得到在关节空间下的封闭形式动力学方程。

由于绳驱机械臂的刚性连杆与柔性绳索之间相互耦合，使得动力学分析较为复杂。为了简化建模难度，将刚性连杆所受作用力按照作用对象分为相邻连杆作用力、惯性力及绳索作用力。基于牛顿-欧拉方程建立机械臂的迭代动力学方程，并对递推形式的动力学方程进行整理，得到封闭形式的动力学方程。

另外，如图2所示，在一实施例中，步骤S110可以包括但不限于以下步骤：

步骤S210，对末端连杆建立力平衡方程，结合D-H坐标系定义的关节转轴方向，得到万向关节的等效驱动转矩；

步骤S220，对其他连杆建立力平衡方程，结合D-H坐标系定义的关节转轴方向，得到万向关节的等效驱动转矩。

首先进行绳驱机械臂的速度与加速度正向递推。根据各个关节的运动得到各个连杆质心坐标系{c_i}的线加速度和角加速度末端连杆只有一个相邻连杆，和其他连杆不同。因此，需要对末端连杆和其他连杆分别建立力平衡方程。

对于末端连杆N，有如下力平衡方程：

其中，表示质心坐标系下{c_N}坐标系相对于{2N}坐标系的旋转矩阵,/>分别为末端连杆N质心坐标系下{c_N}质心的惯性力与惯性力矩，/>分别表示{2N}坐标系中驱动绳索对末端连杆N的作用合力与合力矩，^2Nf_N，^2Nn_N分别为表示在{2N}坐标系下第N连杆受到第N-1连杆的支持力与力矩，/>是在坐标系{2N}表示{2N-2}原点指向质心c_N的位置向量。

结合D-H坐标系定义的关节转轴方向，万向关节的等效驱动转矩τ_N可以由下式计算得到：

其中，驱动力矩分别为/>的y轴与z轴分量,为末端连杆N质心坐标系下{2i}质心的惯性力矩。

对于其余连杆i，同样有力平衡方程：

其中，表示质心坐标系下{c_i}坐标系相对于{2i}坐标系的旋转矩阵，²ⁱR_2i+2表示质心坐标系下{2i+1}坐标系相对于{2i}坐标系的旋转矩阵,/>分别为末端连杆i质心坐标系下{c_i}质心的惯性力与惯性力矩，/>是在坐标系{2i}表示{2i-2}原点指向质心c_i的位置向量，/>分别表示{2i}坐标系中穿行绳索对第i连杆的作用合力与合力矩，/>分别表示{2i}坐标系中驱动绳索对第i连杆的作用合力与合力矩，²ⁱf_i，²ⁱn_i分别为表示在{2i}坐标系下第i连杆受到第i-1连杆的支持力与力矩，²ⁱ⁺²f_i+1，²ⁱ⁺²n_i+1分别为表示在{2i+2}坐标系下第i+1连杆对第i连杆的反作用力与力矩。

通过已知来自第i+1连杆的反作用力和力矩²ⁱ⁺²f_i+1、²ⁱ⁺²n_i+1和所有穿行绳索的作用合力矩可由上式求解得/>从而得到D-H坐标系下的等效驱动转矩τ_i。

为方便表示绳驱机械臂的动力学模型，将推导的递推形式动力学方程进行整理，得到在关节空间下的封闭形式动力学方程：

其中，M，C，g分别为质量、向心力\科氏力和重力矩阵；q_i，分别是关节角，关节角速度和关节角加速度；τ_e表示外力在关节空间产生的转矩，称为外力转矩，τ为关节等效驱动力矩。

关节等效驱动力矩τ与驱动绳索拉力f映射能够整理为以下矩阵形式：

τ＝R_Tf，

其中，R_T为驱动绳索拉力与等效驱动转矩的映射矩阵。

步骤S120，根据封闭形式动力学方程，得到驱动拉力的表达式。

实际对绳驱机械臂进行控制的时候，需要根据实时采集的运动参数进行控制输入的计算，即进行驱动绳索拉力的计算，以使绳驱机械臂的实际运动跟踪期望运动轨迹。由机械臂的运动推导作用力的过程即为逆动力学求解。根据封闭形式动力学方程，驱动拉力可表示为：

其中，R_T ⁺为驱动绳索拉力与等效驱动转矩的映射矩阵的伪逆。

另外，如图3所示，在一实施例中，步骤S120可以包括但不限于以下步骤：

步骤S310，通过绳索拉力分配算法找到可行的解；

步骤S320，根据绳索、关节空间映射矩阵得到驱动绳索拉力与等效关节驱动转矩的标量映射关系。

由于绳驱关节的冗余驱动特性，具体地采用三根绳索驱动两自由度的关节，因此，驱动绳索拉力存在无穷多解，需要通过绳索拉力分配算法找到可行的解。

为简化映射矩阵R_T中较为复杂的空间向量运算，可以借助绳索-关节空间映射矩阵得到驱动绳索拉力与等效关节驱动转矩的标量映射关系。

步骤S130，根据虚功原理，结合关节到绳长的映射关系，得到绳索速度向量、绳索拉力向量、关节速度向量、关节等效力矩的关系；

步骤S140，通过关节转矩计算绳索拉力，驱动拉力解能够表示为最小二乘解与任意零空间解的和。

首先考虑单绳驱关节模型，根据虚功原理，绳索拉力做的功等于外力转矩做的功：

其中，为第i关节驱动绳索速度向量，/>为第i关节驱动绳索拉力向量，/>为第i关节的速度向量，τ_i为第i关节的等效驱动力矩。

结合关节到绳长的映射关系，可以得到：

其中，R_i，i为i组绳索在第i关节段的绳长函数的雅可比矩阵。

由逆运算可以通过关节转矩计算绳索拉力。由于R_i，i ^T为宽矩阵，驱动拉力解可表示为最小二乘解与任意零空间解的和：

其中,N_i＝null(R_i，i ^T)其列空间为矩阵R_i，i ^T的右零空间，即有R_i，i ^TN_i＝0；t_i为内拉力系数，乘积项N_it_i表示驱动绳索的内拉力向量。N_i可以选取为单位向量，则有N_i ^-1＝N_i ^T，则H^-1＝[(R_i，i ^T)⁺N_i]。

另外，如图4所示，在一实施例中，步骤S140可以包括但不限于以下步骤：

步骤S410，选取内拉力系数，内拉力位于绳索在关节段绳长函数的雅可比矩阵的右零空间，内拉力系数任意选取。

由于内拉力位于R_i，i ^T的右零空间，因此内拉力系数t_i能够任意选取，而不会对输出转矩有任何影响。通常最小二乘解得到的绳索拉力并不一定全为正拉力，为防止绳索拉力过小导致绳索松弛，可调节内拉力系数t_i，使得驱动绳索均加上一个正的内拉力，并使其大于设定最小值，保证绳索处于绷紧状态。

另外，如图5所示，在一实施例中，步骤S410可以包括但不限于以下步骤：

步骤S510，求出最小绳索内拉力，使所有绳索拉力均大于拉力下限，得到拉力和的最小解；

步骤S520，求取使各驱动绳索拉力恰好等于拉力下限值的内拉力系数，并取其中的最大值；

步骤S530，使驱动绳索中最小拉力恰好等于拉力下限。

对于内拉力系数t_i的选取有很多种方法，从最小化驱动器输出的角度来说，可以求出最小绳索内拉力，使所有绳索拉力均大于拉力下限，得到拉力和最小解：

其中,f_min为给定的最小绳索正拉力，这里默认所有绳索均相同，采用同一数值；a_k为绳索k拉力的最小二乘解，b_k为绳索k的零空间分量。

在最小绳索拉力的约束下，上式分别求取使得各驱动绳索拉力恰好等于拉力下限值的内拉力系数，并取其中的最大值，使得驱动绳索中最小拉力恰好等于拉力下限，即求得系数为最小正内拉力系数，最终得到的拉力为最小拉力和的解。拉力分配算法能够通过计算内拉力系数保证最小绳索拉力满足拉力下限约束，但是驱动绳索拉力之间的差是由给定输出转矩的大小决定的，因此若有绳索拉力超出上限，即表明在该拉力约束下，给定输出转矩超出关节转矩输出能力。

步骤S150，对绳索拉力约束下的关节转矩输出能力进行分析。

为保证机械臂运行过程中绳索拉力始终在安全约束范围内，需要对最大给定关节转矩进行约束。因此需要对一定绳索拉力约束下的关节转矩输出能力进行分析。

由于绳索拉力的驱动力臂会随关节位置的改变而改变，因此在一定的绳索拉力约束下，关节在不同关节角的极限输出转矩也不同。同时，由于关节的驱动绳索分布位置的方式，在同一关节角下，沿不同转矩输出方向的极限值也会有所差异。

另外，如图6所示，在一实施例中，步骤S150可以包括但不限于以下步骤：

步骤S610，建立坐标系，将输出转矩分解在正交的万向节转轴上；

步骤S620，由万向节转矩得到输出转矩方向；

步骤S630，由绳索拉力约束与输出转矩约束，能够构建优化函数，求取给定关节位置及给定输出转矩方向下的最大输出转矩。

为研究单个关节在给定位置下的不同输出转矩方向的最大输出转矩，建立如图11的坐标系，将输出转矩分解在正交的万向节转轴上。可以由万向节转矩得到输出转矩方向：

其中，τ_x，τ_y分别为输出转矩在万向节转轴上的分量。

由绳索拉力约束与输出转矩约束，可以构建以下优化函数，求取给定关节位置及给定输出转矩方向下的最大输出转矩：

其中，f_min，f_max分别为绳索拉力的下限与上限。

考虑到绳索与绳孔的摩擦，实际的驱动绳索拉力会有所衰减，实际驱动关节的拉力不能达到拉力的上下极限。定义拉力变化系数：

其中，μ为绳索与绳孔的摩擦力系数，α_m为绳索包络角。

另外，如图7所示，在一实施例中，步骤S150可以包括但不限于以下步骤：

步骤S710，假设各驱动绳索均以最大包络角穿行靠近基座端的关节绳孔，假设均采用静摩擦，得到最大拉力变化系数；

步骤S720，根据最大拉力变化系数，经过摩擦力作用后，得到实际驱动拉力。

由于拉力衰减系数与绳索包络角相关，而靠近基座端的各关节角无法确定，也就无法确定实际驱动绳索穿行时的拉力变化系数。为求取驱动拉力，这里作保守的估计，假设各驱动绳索均以最大包络角穿行靠近基座端的关节绳孔，即取最大的拉力变化系数，同时假设为静摩擦。因此最大拉力变化系数为：

其中，α_max为绳索的最大包络角。

映射矩阵R_i，i ^T的各个元素反映了绳索拉力力臂及拉力产生转矩方向。对于第k根驱动绳索，其产生的关节转矩为：

其中，τ_k为第k根驱动绳索产生的关节转矩分量，(r_1k r_2k)^T为映射矩阵R_i，i ^T的第k列元素。

可以通过对转矩向量进行点乘判断绳索拉力对输出转矩是否做正功。并且可以看出，在最小绳索拉力和的分配算法中，最大拉力绳索必定做正功，而最小拉力绳索必定做负功。

经过摩擦力作用后，实际驱动拉力为：

其中，m表示绳索的穿孔次数，对于关节i，有m＝2(i-1)+1，为穿行m次之后拉力变化系数的最大值。

由于最大拉力绳索受摩擦力方向为驱动拉力的反向，因此从驱动箱出来后，绳索拉力经过各导绳孔后逐渐衰减；最小拉力绳索受到摩擦力方向为驱动拉力的同向，因此绳索拉力经过各导绳孔后逐级增加；而另一根绳索拉力大小位于两者之间，无论其摩擦力方向如何，其最终驱动关节的拉力大小也在这两根绳索拉力之间。

也就是说，由于摩擦力的作用，关节驱动端的绳索拉力区间相较于电机端的设定允许拉力区间缩小：

也就意味着关节最大输出转矩也因此减小。通过修改给定关节位置及给定输出转矩方向下的最大输出转矩上下限，可以得到考虑摩擦力影响下，关节最大输出转矩的保守计算。

步骤S160，对绳驱关节的拉力传递过程建立理想数学模型，分析绳索空间拉力控制下的瞬态耦合效应。

在绳驱机械臂逆动力学仿真中，将计算得到绳索的期望驱动拉力直接输入动力学模型中。但在实际系统中，还需要考虑绳索拉力控制的问题。

绳驱关节最直接的控制方式就是采用拉力传感器对每根绳索的拉力进行测量，并单独对每根绳索上的拉力进行独立闭环控制。但是有研究表明，这种关节控制方式会引入瞬态耦合效应，使得不同关节间产生耦合运动。对该现象进行分析，并提出关节空间控制以消除耦合现象。

为分析瞬态耦合的现象，首先对绳驱关节的拉力传递过程进行合理简化以建立理想数学模型。绳驱关节的拉力传递过程是电机驱动减速箱与丝杆，并带动滑块运动使绷紧的绳索发生形变，从而产生拉力的变化，并最终传递到绳驱关节。

另外，如图8所示，在一实施例中，步骤S160可以包括但不限于以下步骤：

步骤S810，忽略传动中摩擦，将传递部件的质量转换至滑块上，简化为滑块驱动绳索的模型；

步骤S820，对绳驱关节的模型作出假设；

步骤S830，根据拉力传递模型，得到驱动电机输出力与滑块运动的关系；

步骤S840，根据绳索刚度模型，通过滑块与关节角的位置差得到绳索拉力。

忽略传动中摩擦，将可以将传递部件的质量转换至移动滑块上，简化为驱动滑块驱动绳索的模型如图12所示。

绳驱关节具有高度非线性的特性，为分析关节的瞬态响应特性，对绳驱关节的模型做以下假设：

绳索-关节速度映射矩阵R为常数。速度映射矩阵会随关节角变化而改变，是一个关于关节角度的非线性函数。在瞬态分析中可以对该函数在初始位置进行线性化处理。经过线性化后可认为R为常数；

连杆的惯性力远小于滑块惯性力。因为所设计机械臂连杆质量小，其惯量远小于驱动机构惯量，即远小于电机转子、减速器、丝杆、滑块等机构的惯量。同时假设臂杆自由运动不受外力影响；

绳索拉力与形变为线性关系。为简化模型，忽略绳索的拉力-形变的非线性因素，使用胡克定律描述绳索刚度模型,

绳索空间拉力控制流程图如图13所示,根据拉力传递模型，驱动电机输出力与滑块运动有以下关系：

s²M_xX(s)＝F_motor(s)-F_t(s)，

其中，X(s)，F_motor(s)，F_t(s)分别表示滑块位置，电机输出力与绳索拉力向量的拉氏变换。其中F_motor(s)＝k_iI(s)，k_i，I(s)分别为电流常数与电流的拉氏变换；M_x为滑块的等效质量矩阵。

根据绳索刚度模型，可以通过滑块与关节角的位置差得到绳索拉力：

f_t＝k_t(x-Rq)，

其中，k_t为绳索刚度系数矩阵，同样假设所有绳索有相同。

另外，如图9所示，在一实施例中，步骤S160可以包括但不限于以下步骤：

步骤S910，对绳索拉力闭环控制中使用PD控制，得到期望电机电流；

步骤S920，得到关节运动与期望转矩输入的关系；

步骤S930，分析得出引起瞬态耦合现象的原因。

可以在对绳索拉力闭环控制中使用PD控制，得到期望电机电流：

其中，k_p，k_d分别为PD控制的比例与微分系数矩阵。

结合上式的线性关系，可以得到：

在等式两边同时左乘R_T，能够得到：

其中,τ(s)为实际关节的输出转矩，考虑关节空间下的动力学，忽略离心力或科氏力项有：

τ(s)＝s²MQ(s)

可以得到关节运动与期望转矩输入的关系：

从上式能够看出，由于分母存在R_TR的非对角矩阵，使得输入期望转矩与关节运动之间产生耦合，即一个关节的期望转矩会作用到其他关节上，从而导致瞬态耦合现象。

关节运动的耦合现象本质上是由于绳驱与关节间的力映射和运动映射关系的差异导致的。而在绳索空间直接进行绳索拉力的闭环无法消除这一差异。要想消除关节运动的瞬态耦合，需要考虑到力映射与运动映射关系并在控制上进行补偿解耦。

步骤S170，进行关节空间拉力控制下的关节运动分析。

为消除关节瞬态耦合，提出绳驱关节空间的转矩控制，关节空间拉力控制流程图如图14所示。

另外，如图10所示，在一实施例中，步骤S170可以包括但不限于以下步骤：

步骤S1010，通过期望与等效实际输出转矩的比较，组成关节转矩的控制闭环；

步骤S1020，通过关节、绳索空间转换得到电机的驱动电流信号，以削弱关节运动耦合效应。

将实际绳索拉力转换为等效实际输出转矩，因此可以通过期望与等效实际输出转矩的比较，组成关节转矩的控制闭环：

其中,为等效关节控制信号。

通过关节-绳索空间转换得到电机的驱动电流信号：

进一步，结合关节动力学，能够得到：

从上式能够看出，由于关节惯量M远小于滑块质量M_x，在二次项中耦合项1/(R_TR)的作用被极大削弱。而对于更高次项，在其系数在刚度系数的倒数1/k_t与关节惯量M的共同作用下被缩小，高次项的耦合作用可忽略。因此关节空间通过关节空间替代绳索空间的拉力控制，可以有效削弱关节运动耦合效应。

步骤S180，在关节空间下建立目标阻抗模型。

阻抗控制的目标是跟踪机器人目标运动轨迹与接触外力之间的阻抗动力学关系，而不是单纯跟踪轨迹。阻抗的动力学可以使用一个质量-阻尼-弹簧二阶系统模型来描述，由三个参数，即惯性系数M_d、阻尼系数D_d、刚度系数K_d决定。调节这三个参数，可以实现不同类型的阻抗特性，如高刚性、高阻尼、高惯性。可以将阻抗控制直观地理解为在机械臂的实际位置与期望位置之间建立一个虚拟的阻抗系统，并控制机械臂跟踪虚拟系统的力输出。由于阻抗系统的“柔顺”性质，使得机械臂运动能够具有柔顺性。

为使得绳驱机械臂关节具有柔顺性，可以在关节空间下建立目标阻抗模型：

其中,Δq为期望关节角与实际关节角之间的误差，为期望关节角速度与实际关节角速度之间的误差，/>为期望关节角加速度与实际关节角加速度之间的误差。

根据绳驱臂的动力学模型，则期望的关节转矩输出为：

由于关节的加速度无法直接测量，无法直接使用该控制律。对控制输出转矩进行简化，并得到如图15所示的关节阻抗控制流程图：

采用阻抗控制律，目标阻抗模型变为：

相比于理想阻抗模型，上式无法跟踪期望的惯性特性，且由于绳驱机械臂的质量较小，该阻抗模型近似当作一阶系统。即便如此，绳驱机械臂任能够具备刚度及阻尼的阻抗特性，且能够规避关节角加速度测量的难题，降低系统复杂度，实用性强。

在本说明书的描述中，若出现参考术语“一个实施例”、“一些实例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上结合附图对本申请的实施方式作了详细说明，但是本申请不限于上述实施方式，在所述技术领域普通技术人员所具备的知识范围内，还可以在不脱离本申请宗旨的前提下作出各种变化。

Claims (10)

1.一种阻抗控制方法，其特征在于：

进行动力学建模，将递推形式动力学方程进行整理，得到在关节空间下的封闭形式动力学方程；

根据封闭形式动力学方程，得到驱动拉力的表达式；

根据虚功原理，结合关节到绳长的映射关系，得到绳索速度向量、绳索拉力向量、关节速度向量、关节等效力矩的关系；

通过关节转矩计算绳索拉力，驱动拉力解能够表示为最小二乘解与任意零空间解的和；

对绳索拉力约束下的关节转矩输出能力进行分析；

对绳驱关节的拉力传递过程建立理想数学模型，分析绳索空间拉力控制下的瞬态耦合效应；

进行关节空间拉力控制下的关节运动分析；

在关节空间下建立目标阻抗模型。

2.根据权利要求1所述的阻抗控制方法，其特征在于，

所述进行动力学建模包括：

对末端连杆建立力平衡方程，结合D-H坐标系定义的关节转轴方向，得到万向关节的等效驱动转矩；

对其他连杆建立力平衡方程，结合D-H坐标系定义的关节转轴方向，得到万向关节的等效驱动转矩。

3.根据权利要求1所述的阻抗控制方法，其特征在于，

所述得到驱动拉力的表达式包括：

通过绳索拉力分配算法找到可行的解；

根据绳索、关节空间映射矩阵得到驱动绳索拉力与等效关节驱动转矩的标量映射关系。

4.根据权利要求1所述的阻抗控制方法，其特征在于，

所述驱动拉力解能够表示为最小二乘解与任意零空间解的和包括：

选取内拉力系数，内拉力位于绳索在关节段绳长函数的雅可比矩阵的右零空间，内拉力系数任意选取。

5.根据权利要求4所述的阻抗控制方法，其特征在于，

所述选取内拉力系数包括：

求出最小绳索内拉力，使所有绳索拉力均大于拉力下限，得到拉力和的最小解；

求取使各驱动绳索拉力恰好等于拉力下限值的内拉力系数，并取其中的最大值；

使驱动绳索中最小拉力恰好等于拉力下限。

6.根据权利要求1所述的阻抗控制方法，其特征在于，

所述对绳索拉力约束下的关节转矩输出能力进行分析包括：

建立坐标系，将输出转矩分解在正交的万向节转轴上；

由万向节转矩得到输出转矩方向；

由绳索拉力约束与输出转矩约束，能够构建优化函数，求取给定关节位置及给定输出转矩方向下的最大输出转矩。

7.根据权利要求6所述的阻抗控制方法，其特征在于，

所述对绳索拉力约束下的关节转矩输出能力进行分析还包括：

假设各驱动绳索均以最大包络角穿行靠近基座端的关节绳孔，假设均采用静摩擦，得到最大拉力变化系数；

根据最大拉力变化系数，经过摩擦力作用后，得到实际驱动拉力。

8.根据权利要求1所述的阻抗控制方法，其特征在于，

对绳驱关节的拉力传递过程建立理想数学模型，分析绳索空间拉力控制下的瞬态耦合效应包括：

忽略传动中摩擦，将传递部件的质量转换至滑块上，简化为滑块驱动绳索的模型；

对绳驱关节的模型作出假设；

根据拉力传递模型，得到驱动电机输出力与滑块运动的关系；

根据绳索刚度模型，通过滑块与关节角的位置差得到绳索拉力。

9.根据权利要求8所述的阻抗控制方法，其特征在于，

对绳驱关节的拉力传递过程建立理想数学模型，分析绳索空间拉力控制下的瞬态耦合效应还包括：

对绳索拉力闭环控制中使用PD控制，得到期望电机电流；

得到关节运动与期望转矩输入的关系；

分析得出引起瞬态耦合现象的原因。

10.根据权利要求1所述的阻抗控制方法，其特征在于，

所述进行关节空间拉力控制下的关节运动分析包括：

通过期望与等效实际输出转矩的比较，组成关节转矩的控制闭环；

通过关节、绳索空间转换得到电机的驱动电流信号，以削弱关节运动耦合效应。