本发明涉及一种机器人关节用挠性驱动单元控制方法，具体涉及一种适用于绳驱动、动 滑轮增力、具有张力和关节位置反馈的机器人关节用挠性驱动单元的控制方法，属于机器人 应用控制领域。

相比刚性驱动，钢丝绳柔性驱动能够吸收震动，减缓冲击，但由于其导致关节回差较大、 且有明显滞后，采用经典PID控制方法效果欠佳。经文献检索发现中国专利公告号为CN 101863034B，专利号为201010210888.3，名称为由滑轮组与钢丝绳牵引驱动的机器人关节用 挠性驱动单元是一种绳驱动关节挠性驱动单元，但没有给出控制方法。申请号为 201410257786.5的一种绳驱动、动滑轮增力、带有张力传感器、关节编码器的机器人关节用 挠性驱动单元没有给出有效的控制方法，无法实现机器人关节用挠性驱动装置张力反馈和关 节全闭环控制，存在控制误差大的问题。

图1是本发明的被控对象虚拟样机图(即申请号为201410257786.5的一种绳驱动、动滑 轮增力、带有张力传感器、关节编码器的机器人关节用挠性驱动单元)，图2是本发明的被控 对象机构原理图(挠性驱动单元机构原理)，图3是本发明的被控对象动力学模型示意图(挠 性驱动单元动力学模型)，图4是本发明设计的控制器框图及其原理图，其中，a)为控制器 框图，b)为控制器原理图；图5为本发明应用实例控制系统硬件系统照片，其中，a)是本发 明应用实例1控制系统硬件系统照片(图中：1-上位机、2-砝码、3-驱动单元、4-加载装置、 5-运动控制卡及A/D、6-驱动器、7-直流开关电源)，b)是本发明应用实例2控制系统硬件系 统照片；图6是本发明控制系统框图；图7-图10是本发明频响测试结果图，其分别是关节理 论与实际转角、关节跟踪误差、关节转角幅值峰值点频率、幅频特性，图7为关节理论与实 际转角对比图，图8为关节跟踪误差图，图9为关节转角幅值峰值点频率图，图10是幅频特 性对比图；图11是关节相位图。

具体实施方式一：如图1～11所示，本实施方式所述的可实现张力反馈、关节位置反馈 的机器人关节用挠性驱动单元控制方法通过挠性驱动单元粘弹性动力学模型进行关节位置和 关节速度反馈，根据钢丝绳弹性变形进行关节速度和关节加速度的前馈。关节位置与速度反 馈，即在电机内部位置环与速度环的基础上引入关节的位置环与速度环，实现挠性驱动单元 关节全闭环控制。对弹性变形的速度与加速度前馈，其基本思想是通过弹性变形公式可求得 钢丝绳伸长量，由伸长量求得需要前馈的速度与加速度，通过实时修改速度前馈系数与加速 度前馈系数来达到补偿钢丝绳伸长量的目的。控制对象为机器人关节用挠性驱动单元，其虚 拟样机如图1所示，其机构原理如图2所示。

可实现张力反馈、关节位置反馈的机器人关节用挠性驱动单元控制方法按如下步骤实现：

步骤一、推导挠性驱动单元粘弹性动力学模型(其示意图如图3所示)，控制系统需要用 到挠性驱动单元关节角公式(即驱动单元粘弹性动力学模型)与钢丝绳弹性变形公式；

减速器输出轴缠绕钢丝绳钢丝绳张力表示如下：

$F_i=-k_i(x_i-r_1{\theta }_1)-c_i({\stackrel{.}{x}}_i-r_1{\stackrel{.}{\theta }}_1)+F_{i,\mathrm{initial}}---\left(7\right)$

其中，下标i＝1，2，其它量见表一，下同。求解上式，取初位移与初速度均为零，得到 减速器输出轴缠绕钢丝绳位移为：

$x_i=e^{-\frac{k_i}{c_i}t}[(r_1{\theta }_1+\frac{F_{i,\mathrm{initial}}}{k_i})(e^{\frac{k_i}{c_i}t}-1)-\frac{1}{c_i}\underset{t_0}{\overset{t}{\int }}{F}_i{e}^{\frac{k_i}{c_i}t}\mathrm{dt}]---\left(8\right)$

其中，i值同上。由动滑轮组增力减速原理有：

$\begin{array}{cccc}{F}_3^{\prime }={4F}_1^{\prime }& F_4^{\prime }=4F_2^{\prime }& x_3=\frac{1}{4}{x}_1& x_4=\frac{1}{4}{x}_2\end{array}---\left(9\right)$

由于减速器端钢丝绳张力未知，而关节端钢丝绳张力可以由张力传感器测得，所以用关 节端张力代替减速器端钢丝绳张力，已知F1,F2,F3,F4与F’1,F’2,F’3,F’4分别互为反力，则 减速器输出轴缠绕钢丝绳位移为如下形式：

$x_i=e^{-\frac{k_i}{c_i}t}[(r_1{\theta }_1+\frac{F_{j,\mathrm{initial}}}{{4k}_j})(e^{\frac{k_i}{c_i}t}-1)-\frac{1}{4c_i}\underset{t_0}{\overset{t}{\int }}{F}_j{e}^{\frac{k_i}{c_i}t}\mathrm{dt}]---\left(10\right)$

其中，下标i＝1，2，j＝3，4。联立公式(9)和(10)，得到关节输出轴缠绕钢丝绳位移为：

$x_j=\frac{1}{4}{e}^{-\frac{k_i}{c_i}t}[(r_1{\theta }_1+\frac{F_{j,\mathrm{initial}}}{{4k}_i})(e^{\frac{k_i}{c_i}t}-1)-\frac{1}{4c_i}\underset{t_0}{\overset{t}{\int }}{F}_j{e}^{\frac{k_i}{c_i}t}\mathrm{dt}]---\left(11\right)$

其中，下标i,j值同上。假定挠性驱动单元关节两侧钢丝绳的拉伸和放松量相同，得到关 节两侧钢丝绳变形约束条件：

x3+x4＝2r2θ2       (13)

联立公式(11)和(13)，得到基于粘弹性动力学的挠性驱动单元关节角公式：

${\theta }_2({\theta }_1,F_3,F_4,t)=\frac{1}{8r_2}\left\{\begin{array}{c}{e}^{\frac{k_1}{c_1}t}[(r_1{\theta }_1+\frac{F_{3,\mathrm{initial}}}{4k_1})(e^{\frac{k_1}{c_1}t}-1)-\frac{1}{4c_1}\underset{t_0}{\overset{t}{\int }}{F}_3{e}^{\frac{k_1}{c_1}t}\mathrm{dt}]+\\ e^{-\frac{k_2}{c_2}t}[(r_1{\theta }_1+\frac{F_{4,\mathrm{initial}}}{4k_4})(e^{\frac{k_2}{c_2}t}-1)-\frac{1}{4c_2}\underset{c_2}{\overset{t}{\int }}{F}_4{e}^{\frac{k_2}{c_2}t}\mathrm{dt}]\end{array}\right\}---\left(14\right)$

当忽略钢丝绳伸长量与单元速度关系时阻尼系数为零，由式(7)求得关节输出轴缠绕钢 丝绳位移为：

$x_j=\frac{1}{4}(r_1{\theta }_1+\frac{F_{j,\mathrm{initial}}-F_j}{{4k}_i})---\left(15\right)$

其中，下标i＝1，2，j＝3，4。可推导出钢丝绳弹性变形为：

△xj＝xj-r2θ2       (16)

其中，下标i,j值同上。联立公式(13)和(15)，得到挠性驱动单元关节角公式如下：

${\theta }_2({\theta }_1,F_3,F_4)=\frac{1}{{8r}_2}({2r}_1{\theta }_1+\frac{F_{3,\mathrm{initial}}-F_3}{{4k}_1}+\frac{F_{4,\mathrm{initial}}-F_4}{{4k}_2})---\left(17\right)$

步骤二、对关节速度与加速度进行前馈；

对关节速度与加速度进行前馈，需要设计前馈控制器，主要根据钢丝绳弹性变形伸长情 况来计算，而弹性伸长通过张力传感器的张力与单元关节角度来进行计算，根据钢丝绳张力 公式(7)可方便求出任意时刻的钢丝绳张力F，该值包含了钢丝绳预紧力、摩擦力和钢丝绳伸 长后的回复力，联立公式(15)和(16)可求出关节两侧钢丝绳弹性变形：

${\mathrm{\Delta x}}_j=\frac{1}{4}(r_1{\theta }_1+\frac{F_{j,\mathrm{initial}}-F_j}{{4k}_i}){-r}_2{\theta }_2---\left(18\right)$      (18)

其中，下标i,j值同公式(16)。由于假定钢丝绳始终处于拉紧状态，因此，△x3和△x4均 大于零，前馈计算需要的钢丝绳弹性伸长对应关节转动方向，该伸长值大于关节转动反方向 的伸长值，那么，在前馈计算首先要根据关节旋转方向判断前馈要使用的钢丝绳弹性伸长值， 判断如下：如果θ2>0(即关节逆时针转动)，那么下标j＝3；否则，j＝4。计算前馈速度与加速 度公式如下：

$\begin{array}{c}\Delta {\stackrel{.}{\theta }}_0=i_m{i}_r\Delta x_j/r_1\mathrm{\Delta T}\\ \Delta {\stackrel{..}{\theta }}_0=\Delta {\stackrel{.}{\theta }}_0/\mathrm{\Delta T}\end{array}---\left(19\right)$    (19)

根据上式可实时修改速度与加速度前馈系数，该系数由当前参考位置和参考速度与前馈 的速度与加速度求得：

$\begin{array}{c}{K}_{\mathrm{vff}}=\Delta {\stackrel{.}{\theta }}_0/{\stackrel{.}{\theta }}_{0,e}\\ K_{\mathrm{aff}}=\Delta {\stackrel{..}{\theta }}_0/{\stackrel{..}{\theta }}_{0,e}\end{array}---\left(20\right)$

步骤三、进行关节全闭环反馈控制。

进行关节全闭环控制，设计反馈控制器，需要的反馈物理量为关节编码器角度值和张力 传感器值，由公式(17)可求出由θ2、r、F3和F4表示的电机角度估计值，如下：

${\hat{\theta }}_0(F_3,F_4,{\theta }_{2,r})=\frac{i_r}{{2r}_1}({8r}_2{\theta }_{2,r}-\frac{F_{3,\mathrm{initial}}-F_3}{{4k}_1}-\frac{F_{4,\mathrm{initial}}-F_4}{{4k}_2})---\left(21\right)$

通过粘弹性动力学模型，根据关节两侧张力值和关节角度值进行相关运算求出电机实际 角度，在此并未直接读取电机编码器角度值进行反馈，目的是将粘弹性动力学模型引入到反 馈控制回路，在后面的实验中，也验证了该方法的可行性。

求出电机角度估计值后，与电机参考值做比较，两者的误差反馈补偿公式如下：

$e_1=K_{P2}({\hat{\theta }}_0-{\theta }_{0,e})---\left(22\right)$

将位置误差补偿到指令电机期望角度值中，相当于电机参考轨迹根据电机实际角度变化 趋势进行实时修改，起到了反馈控制的作用，同时KP2又受到两个因素的影响，即电机实际 速度和钢丝绳张力，根据这两个因素的影响，KP2需要取不同值与其相对应。

将电机角度估计值进行差分计算，得到基于粘弹性动力学模型的电机速度估计值，将该 估计值与电机实际速度做差，则两者的误差反馈补偿公式如下：

$e_2=K_{D2}({\stackrel{.}{\hat{\theta }}}_0-{\stackrel{.}{\theta }}_{0,r})---\left(23\right)$

KD2也受到钢丝绳张力和电机实际速度的影响。比例系数KP2、微分系数KD2根据钢丝绳 张力、电机实际速度进行实时修改，具体调节方法为：获得比例系数KP2、微分系数KD2与钢 丝绳张力、电机实际速度的对应关系作为数据表存入运动控制卡内存，在挠性驱动单元系统 运行时根据采集的钢丝绳张力、电机实际速度调用该数据表从而进行参数的实时修改。