CN112428262B - 基于超椭球映射解析算法并联冗余柔索机构伺服控制方法 - Google Patents

Abstract

本发明涉及基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，包括并联冗余柔索机构反馈控制伪指令与伪指令的解析分配；反馈控制伪指令通过PID控制实现；伪指令解析分配方法通过超椭球映射方法实现。本发明解决了现有并联冗余柔索机构伺服控制系统需要在线优化的问题，反馈控制率由简单矩阵运算组成，构造简单，易于实现，在线求解效率高。

Description

基于超椭球映射解析算法并联冗余柔索机构伺服控制方法

技术领域

本发明属于并联冗余柔索机构伺服控制领，特别涉及基于超椭球映射解析算法并联冗余柔索机构伺服控制方法。

背景技术

相较于传统刚性机构，以柔索张力作为驱动力的并联机构，具有惯性小、体积小、积累误差小与工作空间大的特点，能够最大限度地满足装备质量轻、控制精度高与任务包络大等需求，目前广泛应用于医疗康复、工业机器人等领域。并联冗余柔索机构是指控制柔索数量大于被控自由度数量的一类柔索驱动机构，其需要通过控制分配环节将低维反馈控制指令转化为高维的张力分配以实现位、姿控制。目前，并联冗余柔索机构的控制分配算法也可大致分为两类：

第1类，优化法。该类方法能够寻找到全局最优解，但是极限计算消耗(最大迭代次数)在机构工作过程中不受控。第2类，几何法。相比于优化法，几何法无法找到全局最优解、最大迭代次数可控。该类方法的提出就是为了保证在线求解时最大迭代次数可控。但是，目前现有控制方法仅能保证最大在线迭代次数可控，无法消除在线迭代计算的环节，降低了反馈控制率求解效率。

目前针对并联冗余柔索机构的控制器设计研究领域，目前研究热点为各类控制律与控制分配优化或几何迭代算法的组合，例如通过自适应鲁棒控制与控制分配优化法相结合、控制与控制分配几何迭代方法相结合以及鲁棒PID控制与控制分配几何迭代方法相结合等。以上研究方法所基于的控制分配算法成熟，在线控制分配算法对于计算能力依赖较大，目前为并联冗余柔索机构控制器设计的主流方法。另外，由于目前控制分配的解析算法研究仍然没有完善成熟的方法，对于控制率的解析算法目前还处于尝试阶段，虽然能够显提高低控制响应速度，降低控制系统复杂度，但是存在控制张力存在浪费问题。

发明内容

本发明解决的技术问题是：为克服现有技术的不足，提出基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，将PID反馈控制与超椭球映射解析分配算法相结合，解决现有并联冗余柔索机构主动控制系统需要在线迭代所导致的控制率求解效率问题。

本发明解决技术的方案是：

基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，该方法的步骤包括：

步骤(1)、确定并联冗余柔索机构CDPR运动部件的质量m、转动惯量I_xx、I_yy与I_zz，定义状态向量为x＝(p^T,Θ^T)^T，其中p＝(p_x,p_y,p_z)^T为CDPR运动部件质心位移向量，

为并联冗余柔索机构运动部件姿态角向量，

为滚动角、θ为俯仰角、ψ为偏航角；定义PID控制参数矩阵K_p、K_I与K_D，控制参数矩阵维度为6×6；

步骤(2)、通过传感器提取与处理信息，获得并联冗余柔索机构运动部件状态信息x，确定该时刻目标轨迹x_d，求解运动误差e＝x_d-x；

步骤(3)、根据步骤(2)所获得的运动部件状态信息向量x，求解并联冗余柔索机构的雅可比矩阵J，雅可比矩阵维度为6×m，m≥6为控制柔索数量；

步骤(4)、通过步骤(1)所获得的m、I_xx、I_yy、I_zz与步骤(2)所获得的并联冗余柔索机构状态信息x、运动误差e，求解伺服控制伪指令f，伺服控制伪指令的维度为6×1；

步骤(5)、通过步骤(2)所获得的并联冗余柔索机构状态信息x与步骤(3)所获得的雅可比矩阵J求解控制张力最小超球直径r；

步骤(6)、根据步骤(4)所获得的伺服控制伪指令与步骤(5)所获得的控制张力超球最小直径r，求解控制张力分配向量T，控制张力分配向量维度为m×1，将控制张力分配向量T分配到柔索驱动电机执行；

步骤(7)、运行时间每间隔控制步长Δt，重复步骤(2)至(6)直至并联冗余柔索机构完整生成目标运动轨迹。

进一步的，步骤(3)中的雅可比矩阵J通过下式进行求解

上式中u_i为全局坐标系下第i根柔索的单位方向向量，b_i为并联冗余柔索机构运动部件随体坐标系下运动部件质心与第i根柔索连接点间的向量，R为姿态角转换矩阵；

进一步的，R通过任意转换方法，以3-2-1转换顺序，通过下式进行求解

其中c_α与s_α分别表示cos(α)与sin(α)。

4、根据权利要求1所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：步骤(4)中的伺服控制伪指令f通过下式进行求解：

其中M(x)为质量矩阵，

为柯氏力与向心力矩阵，x为状态变量，

为状态变量速度，

为目标轨迹加速度，g为外部力与扭矩所组成的外载荷向量；

为运动误差对时间的一阶导；

进一步的，

其中Q为姿态角速度转换矩阵，m为质量，I_xx、I_yy、I_zz为x、y、z三个方向上的转动惯量；

为目标姿态角向量对时间的一阶导。

进一步的，以3-2-1转换顺序，通过下式进行求解，

进一步的，步骤(5)中的控制张力最小超球半径r通过下式进行求解

其中a、b与c为中间变量。

进一步的，

b＝I₃₁J^T(JJ^T)f,c＝f^T(JJ^T)f,

进一步的，步骤(6)中的控制张力分配向量T通过下式进行求解

T＝-J^T(JJ^T)f+rηη^TI₃₁

其中η为矩阵-J的零空间。

本发明与现有技术相比的有益效果是：

(1)本发明采用基于超椭球映射解析算法的并联冗余柔索机构伺服控制方法，控制率可以通过解析过程进行求解，无需在线优化，求解速度快；

(2)本发明所述的超椭球映射解析分配算法所求解的控制张力连续、可导，能够有效抑制控制分配优化不连续导致的高频振动；

(3)本发明反馈控制率构造简单，具有较好的可移植性，能够与任意任意并联冗余柔索机构主动控制系统相结合。

附图说明

图1是本发明流程图；

图2是本发明实施例中的结构尺寸、载荷与约束环境；

图3是本发明不考虑载荷、观测与致动误差的机构运动误差；

图4是本发明不考虑载荷、观测与致动误差的柔索控制张力；

图5是本发明考虑载荷、观测与致动误差的机构运动误差；

图6是本发明考虑载荷、观测与致动误差的柔索控制张力。

具体实施方式

下面结合实施例对本发明作进一步阐述。

如图1所示，本发明提出了基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，包括以下步骤：

步骤(1)、确定并联冗余柔索机构(CDPR,Cable-Driven Parallel Robots)运动部件的质量m、转动惯量I_xx、I_yy与I_zz，定义状态向量为x＝(p^T,Θ^T)^T，其中p＝(p_x,p_y,p_z)^T为CDPR运动部件质心位移向量，

为并联冗余柔索机构运动部件姿态角向量，

为滚动角、θ为俯仰角、ψ为偏航角；定义PID控制参数矩阵K_p、K_I与K_D，控制参数矩阵维度为6×6；

步骤(2)、通过传感器提取与处理信息，获得并联冗余柔索机构运动部件状态信息x，确定该时刻目标轨迹x_d，求解运动误差e＝x_d-x；

步骤(3)、根据步骤(2)所获得的运动部件状态信息向量x，在线求解并联冗余柔索机构的雅可比矩阵

上式中u_i为全局坐标系下第i根柔索的单位方向向量，b_i为并联冗余柔索机构运动部件随体坐标系下运动部件质心与第i根柔索连接点间的向量，R为姿态角转换矩阵，可以通过任意转换方法，以3-2-1转换顺序为例，可通过下式进行求解

其中c_α与s_α分别表示cos(α)与sin(α)；

步骤(4)、通过步骤(1)所获得的m、I_xx、I_yy、I_zz与步骤(2)所获得的并联冗余柔索机构状态信息x、运动误差信息e求解伺服控制伪指令

其中M(x)为质量矩阵，

为柯氏力与向心力矩阵，分别通过如下式进行求解

其中Q为姿态角速度转换矩阵，根据根据权利要求3中姿态角转换顺序确定，以3-2-1转换顺序为例，可通过下式进行求解

步骤(5)、通过步骤(2)所获得的并联冗余柔索机构状态信息x与步骤(3)所获得的雅可比矩阵J求解控制张力最小超球直径

其中a、b与c为中间变量，需通过如下式进行求解

b＝I₃₁J^T(JJ^T)f,c＝f^T(JJ^T)f,

步骤(6)、根据步骤(4)所获得的伺服控制伪指令与步骤(5)所获得的控制张力超球最小直径r求解控制张力分配向量

T＝-J^T(JJ^T)f+rηη^TI₃₁

其中η为矩阵-J的零空间，将控制张力分配向量T分配到柔索驱动电机执行；

步骤(7)、运行时间每间隔控制步长Δt，重复步骤(2)至(6)直至并联冗余柔索机构完整生成目标运动轨迹。

实施例

为了充分地了解本发明的特点以及其对于工程实际的适用性，本发明建立如图2所示的并联冗余柔索机构与载荷约束环境，2自由度冗余空间并联柔索机构，基本尺寸如下表所示：

目标轨迹为起始点与终止点之间的直线连线，其中位移轨迹如下式

其中T＝1s,T_a＝0.5s,T_c＝0s,与代表了完整运动时间、加速时间、匀速时间，v₂为匀速段速度。起始点与终止点状态信息如下表

为了更加准确的描述机构伺服控制过程中遇到的实际环境，附加边界最大为[-0.3,+0.3]的载荷扰动，附加边界最大为[-0.01,+0.01]的观测误差，附加边界最大为[-0.3,+0.3]的致动误差。实施例采用数值实验方法，计算步长Δt为0.0001s，控制延迟0.0001s。

通过图3可以看出，当不考虑不考虑载荷、观测与致动误差的情况下，机构运动误差量级很小，在0.5s左右会出现驻点或峰值点，原因在于实施例中0.5s时出现了加速度方向的转变。

通过图4可以看出，当不考虑载荷、观测与致动误差时，各个柔索控制张力连续可导，说明本专利提出的控制方法能够有效消除跟踪误差。

通过图5可以看出，当考虑载荷、观测与致动误差时，机构跟踪误差仍然处于较小量级，没有出现发散情况。

通过图6可以看出，控制张力由于在随机不确定性因素作用下，在实时调整，说明本专利提出的控制方法在存在载荷、观测与致动误差时，仍然能够有效消除跟踪误差。

本发明虽然已较佳实例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不拓扑本发明的精神和范围内，都可以领上述揭示的方法和技术对本发明技术方案作出可能的变动和修改。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例作的任何简单修改等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (6)

1.基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于，该方法的步骤包括：

步骤(1)、确定并联冗余柔索机构运动部件的质量m、转动惯量I_xx、I_yy与I_zz，定义状态向量为x＝(p^T,Θ^T)^T，其中p＝(p_x,p_y,p_z)^T为并联冗余柔索机构运动部件质心位移向量，

为并联冗余柔索机构运动部件姿态角向量，

为滚动角、θ为俯仰角、ψ为偏航角；定义PID控制参数矩阵K_p、K_I与K_D，控制参数矩阵维度为6×6；

步骤(2)、通过传感器提取与处理信息，获得并联冗余柔索机构运动部件状态信息x，确定该时刻目标轨迹x_d，求解运动误差e＝x_d-x；

步骤(3)、根据步骤(2)所获得的运动部件状态信息向量x，求解并联冗余柔索机构的雅可比矩阵J，雅可比矩阵维度为6×m，m≥6为控制柔索数量；

步骤(4)、通过步骤(1)所获得的m、I_xx、I_yy、I_zz与步骤(2)所获得的并联冗余柔索机构状态信息x、运动误差e，求解伺服控制伪指令f，伺服控制伪指令的维度为6×1；

步骤(5)、通过步骤(2)所获得的并联冗余柔索机构状态信息x与步骤(3)所获得的雅可比矩阵J求解控制张力最小超球直径r；

步骤(6)、根据步骤(4)所获得的伺服控制伪指令与步骤(5)所获得的控制张力超球最小直径r，求解控制张力分配向量T，控制张力分配向量维度为m×1，将控制张力分配向量T分配到柔索驱动电机执行；

步骤(7)、运行时间每间隔控制步长Δt，重复步骤(2)至(6)直至并联冗余柔索机构完整生成目标运动轨迹；

步骤(4)中的伺服控制伪指令f通过下式进行求解：

其中M(x)为质量矩阵，

为柯氏力与向心力矩阵，x为状态变量，

为状态变量速度，

为目标轨迹加速度，g为外部力与扭矩所组成的外载荷向量；

为运动误差对时间的一阶导；

步骤(5)中的控制张力最小超球半径r通过下式进行求解

其中a、b与c为中间变量；

步骤(6)中的控制张力分配向量T通过下式进行求解

T＝-J^T(JJ^T)f+rηη^TI₃₁

其中η为矩阵-J的零空间。

2.根据权利要求1所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：步骤(3)中的雅可比矩阵J通过下式进行求解

上式中u_i为全局坐标系下第i根柔索的单位方向向量，b_i为并联冗余柔索机构运动部件随体坐标系下运动部件质心与第i根柔索连接点间的向量，R为姿态角转换矩阵。

3.根据权利要求2所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：R通过任意转换方法，以3-2-1转换顺序，通过下式进行求解

其中c_α与s_α分别表示cos(α)与sin(α)。

4.根据权利要求1所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：

其中Q为姿态角速度转换矩阵，m为质量，I_xx、I_yy、I_zz为x、y、z三个方向上的转动惯量；

为目标姿态角向量对时间的一阶导。

5.根据权利要求1所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：以3-2-1转换顺序，通过下式进行求解，

6.根据权利要求1所述的基于超椭球映射解析算法并联冗余柔索机构伺服控制方法，其特征在于：

b＝I₃₁J^T(JJ^T)f,c＝f^T(JJ^T)f,

Images