[发明专利]一种联动型绳驱柔性机械臂的闭环控制方法和系统

权利要求书

1.一种联动型绳驱柔性机械臂的闭环控制方法，用于控制联动型绳驱柔性机械臂执行作业任务，其特征在于，所述方法包括以下步骤：

S110、获取所述联动型绳驱柔性机械臂的下一时刻的期望目标构型q_d；

S120、获取所述联动型绳驱柔性机械臂的所有角度传感器数据q_real；

S130、计算当前时刻各个所述联动臂段的等效关节转角q_eq；

S140、将所述期望目标构型q_d与各个所述联动臂段的等效关节转角q_eq的差值作为关节闭环控制输入量，计算出关节闭环输出量PID_out；

S150、将所述关节闭环输出量PID_out转换为联动型绳驱柔性机械臂绳索的运动速度量Vel_l；

S160、将所述联动型绳驱柔性机械臂绳索的运动速度量Vel_l转化为电机运动速度；

S170、控制电机跟随电机运动速度进行运动；

S180、判断当前联动型绳驱柔性机械臂的构型q_cur是否为最终目标期望构型q_tar，若否则返回S110进入下一个控制周期，若是则结束控制。

2.根据权利要求1所述的方法，其中，在所述步骤S110前还包括：

S101、根据关节角度传感器获得当前联动型绳驱柔性机械臂的构型q_cur，以及根据用户输入获得最终目标期望构型q_tar，将所述构型q_cur和所述最终目标期望构型q_tar输入到轨迹规划器，所述轨迹规划器包括但不限于五次样条规划器、三次样条规划器和梯形规划器；

S102、获取由所述轨迹规划器输出一系列随时间变化的构型状态点得到一条轨迹，根据控制周期T把所述轨迹从所述当前联动型绳驱柔性机械臂的构型q_cur到最终目标期望构型q_tar离散化为一系列轨迹点，所述当前联动型绳驱柔性机械臂的构型q_cur为一系列轨迹状态点中的第一个，所述联动型绳驱柔性机械臂的目标期望轨迹构型q_tar为由联动型绳驱柔性机械臂各关节转角描述的一系列轨迹状态点中最后一个，所述关节转角为联动型绳驱柔性机械臂相邻关节与臂杆间绕转轴轴线的相对角度。

3.根据权利要求1所述的方法，其中，所述步骤S110包括：

通过所述轨迹规划器获取所述联动型绳驱柔性机械臂的构型时间曲线，进而得到每一个控制周期T的期望构型，

使所述联动型绳驱柔性机械臂设置的轨迹构型为：

t_tar＝[t₁，t₂，...，t_n]为与所述轨迹构型关联的时间矩阵，

在子区间t∈[t_i，t_i+1]中，插值后的构型时间运动曲线为：

q_d＝a_i+b_i(t_d-t_i)+c_i(t_d-t_i)²+d_i(t_d-t_i)³

其中，

a_i＝q_i

其中，t_d为期望构型q_d的所在时刻。

4.根据权利要求1所述的方法，其中，所述步骤S130、计算当前时刻各个所述联动臂段的等效关节转角q_eq，包括，

所述等效关节等效转角q_eq为该所述联动臂段内各关节转角的所有俯仰角度的加权平均值或所有偏航角度的平均值。