CN115431263A - 一种复杂执行器故障下多冗余机械臂系统协同控制方法 - Google Patents

Abstract

本发明提供一种复杂执行器故障下多冗余机械臂系统协同控制方法，包括：机械臂运动约束算法设计，所述机械臂运动约束算法采用渐进最优的RRT*算法，由碰撞检测、轨迹规划和正逆动力学构成；复杂执行器故障下多冗余机械臂系统动力学模型设计；分布式事件触发状态观测器设计，所述分布式事件触发状态观测器包括通信拓扑结构、事件触发机制、分布式观测器；自适应补偿控制器设计，所述自适应补偿控制器采用反步法实现，通过构造闭环系统的Lyapunov函数来获得反馈控制器，并引入Nussbaum函数和虚拟控制律。本发明可以有效消除复杂故障对执行器的影响，保证设备的正常稳定运行。

Description

一种复杂执行器故障下多冗余机械臂系统协同控制方法

技术领域

本发明涉及多冗余机械臂运动控制领域，具体涉及一种复杂执行器故障下多冗余机械臂系统协同控制方法。

背景技术

机械臂是一个典型的多输入多输出的强耦合复杂系统，其在工业上的应用已有六十多年的历史。如今，随着智能技术的不断发展，图像识别、深度学习等人工智能技术已经广泛应用于机械臂领域，使得机械臂的应用场景逐渐从传统工业领域向服务行业渗透：IBM、麻省理工学院和Heartport公司联手研发的达芬奇手术机器人在医疗领域可代替医生对患者进行手术治疗；Franka Emika公司研发的七自由度冗余机械臂Panda配备高精度力传感器，为研究人员提供了可靠的开发平台。

与单个机械臂相比，多冗余机械臂系统具有更大的灵活性、更强的可靠性以及更高的运行效率，能够完成更复杂的任务。但相应地，由于结构的复杂性，多冗余机械臂系统的运动学和动力学的分析变得更加繁琐和复杂。在运动学层面上，多冗余机械臂系统基坐标系标定需要各机械臂的相对位置，且多冗余机械臂系统的运动轨迹规划相比于单个机械臂更为复杂。换句话说，多冗余机械臂系统在执行复杂的协同任务时，除了要考虑各机械臂自身位姿约束之外，还要考虑协作机械臂间的相对运动约束，这使得运动学分析更加困难。在动力学层面上，由于多冗余机械臂系统是一个自由度高度冗余的机械系统，该特性让机械臂的耦合关系更加复杂，同时系统中不同构型机械臂的动力学参数也会不同，这就使得传统的单机械臂控制算法无法直接运用到多冗余机械臂系统中。因此必须深入分析多冗余机械臂系统在运动学和动力学的性质，建立能够描述整个协同系统动力学特性的数学模型，才能实现高效的协同控制目标。

在实际工况下，多冗余机械臂系统的执行器一直处于运行状态，这使得其成为系统中最容易发生故障的部件。常见的执行器故障有卡死、部分失效、振荡和存在噪音干扰等。与网络层的网络攻击不同，执行器故障在物理层非常常见，通常是由于设备老化和运行异常所致。机械臂的执行机构为驱动电机。在执行器发生故障时，执行器上施加的实际转矩是未知的，除了机械臂电机提供的额定转矩之外，实际转矩还受故障的影响。执行器故障会改变机械臂的运行轨迹，从而达不到预期的控制目标。目前，大多数研究一般考虑到以下两种情况的执行器故障：一是执行器发生部分失效故障，二是执行器发生卡死故障。解决上述故障的常用方法是设计自适应补偿律消除故障对执行器的影响，以保证设备的正常运行。该方法不需要故障诊断和隔离单元，可以避免故障诊断误差的影响，且保证了系统的实时性。然而正如之前描述的那样，研究中的执行器故障并没有考虑到控制方向未知这种情况，驱动电机振荡和反转都会造成控制方向的突变。除此之外，多冗余机械臂系统是一个典型的多输入多输出(MIMO)系统，这就要求设计更为合适的自适应补偿控制律以确保系统的稳定性。由此可见，在更加复杂的执行器故障情况下，开发适用于MIMO系统的自适应补偿控制算法尤为重要。

另一方面，在早期的研究中，多机械臂体系统通常假定实时连续的通信。然而，在实际情况下，每个智能体配备的是能量有限的嵌入式微处理器。因此，如何在有限的能量和通信资源下，实现信息采集、与邻近区域通信、产生控制信号等功能来驱动系统具有重要意义。对多冗余机械臂系统而言，传统的控制以周期的方式执行，即所谓的时间触发控制，基于采样数据系统理论，具有可预测性，易于实现。但是，从资源利用的角度看，时间触发控制周期地执行控制任务会浪费计算和设备能源等资源。此外，如果采样周期比较小，则大量冗余采样信号将被释放到带宽有限的共享通信网络中，这必然会造成网络拥塞。因此，事件触发控制引起了越来越多学者的关注。所谓事件触发控制，是指控制任务是否执行由事先给定的事件触发条件决定，而不是根据时间情况。如果触发条件在某一时刻满足，则意味着事件触发，立即执行控制任务。与时间触发控制机制相比，事件触发控制方案可以减少计算资源、电池装置能源和通信资源的使用。事实上，一些实验已经证明事件触发控制方法可以有效地减少控制任务执行数量，从而在保证闭环系统性能的基础上，显著地节约通信资源。

发明内容

有鉴于此，本发明提供一种复杂执行器故障下多冗余机械臂系统协同控制方法，包括以下步骤：

S1、机械臂运动约束算法设计，所述机械臂运动约束算法采用渐进最优的RRT*算法，由碰撞检测、轨迹规划和正逆动力学构成；

S2、复杂执行器故障下多冗余机械臂系统动力学模型设计；

S3、分布式事件触发状态观测器设计，所述分布式事件触发状态观测器包括通信拓扑结构、事件触发机制、分布式观测器；

S4、自适应补偿控制器设计，所述自适应补偿控制器采用反步法实现，通过构造闭环系统的Lyapunov函数来获得反馈控制器，并引入Nussbaum函数和虚拟控制律。

本发明提供的技术方案带来的有益效果是：

运动学层面上，本发明考虑机械臂自身位姿约束和多个机械臂间的相对运动约束，运用RRT*算法，让机械臂在线寻找避障路径；在动力学层面，本发明设计基于Nussbaum函数的自适应补偿控制算法去解决复杂执行器故障对多冗余机械臂系统的影响，保证整个闭环系统的稳定，并引入事件触发机制节省通信资源。本发明可以有效消除复杂故障对执行器的影响，提高多冗余机械臂系统的鲁棒性，保证设备的正常稳定运行。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明的一种复杂执行器故障下多冗余机械臂系统协同控制方法实施方案的框架图；

图2是本发明的一种复杂执行器故障下多冗余机械臂系统协同控制方法所提出的多冗余机械臂系统通信拓扑结构；

图3是本发明的一种复杂执行器故障下多冗余机械臂系统协同控制方法所提出的渐进最优的RRT*算法重新选择父节点过程示意图；

图4是本发明的一种复杂执行器故障下多冗余机械臂系统协同控制方法所提出的渐进最优的RRT*算法重新布线随机树的过程示意图；

图5是本发明的一种复杂执行器故障下多冗余机械臂系统协同控制方法所提出的分布式观测器位置估计曲线；

图6是本发明的一种复杂执行器故障下多冗余机械臂系统协同控制方法所提出的分布式观测器速度估计曲线；

图7是本发明的一种复杂执行器故障下多冗余机械臂系统事件触发协同控制系统位置曲线；

图8是本发明的一种复杂执行器故障下多冗余机械臂系统事件触发协同控制系统速度曲线；

图9是本发明的一种复杂执行器故障下多冗余机械臂系统事件触发协同控制系统位置误差曲线；

图10是本发明的一种复杂执行器故障下多冗余机械臂系统事件触发协同控制系统速度误差曲线；

图11是本发明的一种复杂执行器故障下多冗余机械臂系统协同控制方法所提出的RRT*算法在三维空间中的演示图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地描述。

一种复杂执行器故障下多冗余机械臂系统协同控制方法，包括以下步骤：

S1、机械臂运动约束算法设计，所述机械臂运动约束算法采用渐进最优的RRT*算法，由碰撞检测、轨迹规划和正逆动力学构成，具体为：

具体为：

S11、机械臂初始点作为根节点x_init，由此开始生成枝丫；

S12、在机器人的“构型”空间中，生成一个随机点x_rand；

S13、遍历树中的点，找到离x_rand最近的点x_near；

S14、从x_near向x_near方向按一定的步长生长，如果没有碰到障碍物就把生长后的树枝和端点添加到树上，记为x_new，

碰撞条件为：

x₀-r₀-l＜x_new,x＜x₀+r₀+l

y₀-r₀-l＜x_new,y＜y₀+r₀+l

z₀-r₀-l＜x_new,z＜z₀+r₀+l

其中，(x₀,y₀,z₀)为球形障碍物的球心坐标，r₀为球径，l为膨化处理后障碍物的膨胀尺寸；

S15、在新产生的节点x_new附近定义一个半径为r的“区域”，区域中的节点作为x_new父节点的备选

S16、依此计算所有x_init到

的代价与

到x_new的代价和，选取代价和最小的

作为x_new父节点

S17、将剩余的备选节点

的父节点改变为x_new，计算x_init到x_new的代价与x_new到

的代价和；

S18、如果代价和小于之前未更改父节点的代价和，则该

节点的父节点更改为x_new；

S19、重复S12-S18过程，直到到达目标节点x_goal。

步骤S12-S16是重新选择父节点的过程，过程的具体示意图如图3所示；步骤S17-S18是重新布线随机树的过程，过程的具体示意图如图4所示。

S2、复杂执行器故障下多冗余机械臂系统动力学模型设计，具体为：

S21、定义机械臂i的第m个执行器的故障模型为：

其中，μ_i,m表示由执行器故障引起的非零有界比例系数，τ_i,m(t)表示机械臂i的第m个执行器的控制输入，

表示执行器中的有界故障信号，上述故障模型涵盖的执行器故障为：当μ_i,m＝1且

时，系统中的机械臂i的第m个执行器存在噪音干扰；当0＜μ_i,m＜1且

时，系统中的机械臂i的第m个执行器发生部分失效故障；当μ_i,m＜0且

时，系统中的机械臂i的第m个执行器的控制方向发生突变；

S22、采用拉格朗日方程对故障下多冗余机械臂系统动力学进行建模，具体模型如下：

其中，q_i，

分别表示机械臂i的关节位置，关节速度和关节加速度，本设计中的多冗余机械臂系统由i个7自由度冗余机械臂构成，即执行器故障模型中m＝1,…,7，M_i(q_i)∈R^7×7为对称正定惯性矩阵，

为哥氏力和向心力矩阵，G_i(q_i)∈R⁷为重力作用矩阵，

为机械臂i各关节执行器故障后的输出转矩，J_i(q_i)∈R^7×7为反应机器人关节速度到末端执行器速度变换关系的雅可比矩阵，F_ei∈R⁷为机器人末端的输出作用力；

令

则上述多冗余机械臂系统动力学模型表示为：

其中

变换后的模型将在自适应补偿控制算法设计中使用。

S3、分布式事件触发状态观测器设计，所述分布式事件触发状态观测器包括通信拓扑结构、事件触发机制、分布式观测器。具体为：

S31、设计多冗余机械臂系统的通信拓扑结构，所述通信拓扑结构用一个加权有向图表示，记为G≡(V,E,A)，其中V＝{v₁,v₂,...,v_N}表示一组节点，节点v_i代表第i个机械臂；

表示边集合，当节点i向节点j发送信息，则边(v_j,v_i)∈E；A＝[a_ij]_N×N表示一个邻接矩阵，其中a_ij表示边(v_i,v_j)的权重，即节点i接收到节点j的信息，当a_ij＝0，则边(v_i,v_j)不存在，参考信号由一个虚拟的线形系统生成，标记为节点v₀，其动力学模型如下：

其中

是领导节点的状态，表示机械臂关节空间的参考信号，q₀为参考关节位置，

为参考关节速度。A∈R^14×14是一个已知的常数矩阵，其形式为一个分块矩阵，即A＝block{0₇,I₇；A₀,A₁}，0₇表示一个7×7的全零矩阵，I₇表示一个7×7的单位矩阵，A₀和A₁表示一个7×7的常数矩阵，领导节点可以被有向图G中的一部分节点观测。如果节点i可以观测领导者，则存在一条带有加权增益gi>0的边(vi,v₀)连接彼此。我们则称节点i为受控节点，并定义受控矩阵为G＝diag{g_i}∈R^N×N。本实施例中令N＝5，即多冗余机械臂系统中有5个7自由度冗余机械臂，其拓扑结构图如图2所示，该有向图至少存在一个以节点v0为根节点的生成树。

S32、假设机械臂1～机械臂5要跟踪一个参考信号，由于通讯拓扑结构的限制，并不是每个机械臂都可以直接获取参考信号的相关信息，所以需要设计分布式状态观测器使得每个机械臂利用与其直接相连的邻居机械臂去估计参考信号的相关信息。分布式事件触发状态观测器设计如下：

其中i＝1,...,5，β_i＞0，

表示机械臂i对关节空间的参考信号的估计，

为关节位置估计值，

为关节速度估计值，

表示机械臂i的触发时刻，且

S33、引入事件触发条件，定义了如下新变量：

其中

为机械臂i的邻居误差，代表机械臂i可以接收的信息综合误差，

为经过事件触发后的机械臂i的邻居误差。触发时刻是由事件触发机制生成的，基于上述定义的新变量，设计一个事件触发条件来确定何时应该传输观测器估计状态，并将触发后状态作为控制器的状态变量使用，具体的事件触发机制如下：

其中

θ_i和δ_i是给定的正常数，分别反映

的幅值和趋于零的速率。

S4、自适应补偿控制器设计，所述自适应补偿控制器采用反步法实现，通过构造闭环系统的Lyapunov函数来获得反馈控制器，并引入Nussbaum函数和虚拟控制律。具体为：

S41、所述控制器的误差矢量定义如下：

其中α_i＝-c₁z_i1是虚拟控制律，且c₁＞0；

S42、设计Nussbaum函数如下：

S43、设计Lyapunov函数：

为信号

估计误差

d_i,m为

的上上确界

为d_i,m的估计值；

S44、设计自适应补偿控制算法：

其中

且ζ_ψ,

c₁,c₂均为正常数。

本实施例中为了验证设计的自适应补偿控制器使多冗余机械臂系统在复杂执行器故障的情况下，各机械臂的所有状态能够跟随给定的参考信号变化，选取以下动力学模型参数：M_i(x_i1)为7维单位矩阵，Q_i(x_i1,x_i2)＝-ones(7,1)为元素全为-1的7维列向量，μ_i,m＝-1.5，

参考信号的状态矩阵选取为：A＝block{0₇,I₇；A₀,A₁}，其中A₀＝-I₇，A₁＝0₇，初始值为

表示元素全为1的7维列向量。分布式事件触发观测器中，β_i＝150，θ_i＝0.3且δ_i＝0.5。自适应补偿控制器的相关参数为：ζ_ψ＝0.5，

c₁＝5，c₂＝15，其他参数的初始值都为0。仿真结果如图5～图11所示，总时间为15s，步长为0.001s，总步数为15000。

虽然参考信号的位置和速度都是7维向量，但由于关节参考给定曲线以及不同机械臂的参数设置都相同，唯一不同的地方是通信拓扑结构。为了直观展示效果，图中仅用一个关节的位置和速度代表所有机械臂的位置和速度向量。

图5和图6是观测器位置和速度估计曲线，从图中可以看出，所设计的分布式事件触发状态观测器在5s之后能够使各个机械臂跟踪参考信号，且跟踪误差极小。在0～5s时，跟踪曲线呈阶梯变换，这是事件触发机制造成的。在观测器中加入计数器，测得各机械臂的事件触发次数为8908，8944，8860，8860，8860，节省了约40％的通信资源。故可以说明所设计的分布式事件触发状态观测器的可靠性和实用性。

图7、图8是引入自适应补偿控制器后系统的位置和速度曲线，可以直观地看出，系统位置和速度能够快速跟踪参考信号。这里定义速度和位置误差为：

e_i1＝x_i1-q₀

其中i＝1,...,5。系统位置误差和速度误差曲线如图9和图10所示，从图中可以看出，位置误差最终在10^-5数量级，速度误差也在10^-2数量级，故可以说明整体方案的有效性和可靠性。

图11是RRT*算法在三维空间中的演示图，起始点的坐标是(150,150,150)，目标点的坐标是(10,10,10)，空间中设置5个球形障碍物。从图中可以看出，在有限次的迭代过程后，使用RRT*算法可以做到三维空间的球形避障任务。

Claims (5)

1.一种复杂执行器故障下多冗余机械臂系统协同控制方法，其特征在于，包括以下步骤：

S1、设计机械臂运动约束算法，所述机械臂运动约束算法采用渐进最优的RRT*算法，由碰撞检测、轨迹规划和正逆动力学构成；

S2、设计复杂执行器故障下多冗余机械臂系统动力学模型；

S3、设计分布式事件触发状态观测器，所述分布式事件触发状态观测器包括通信拓扑结构、事件触发机制、分布式观测器；

S4、设计自适应补偿控制器，所述自适应补偿控制器采用反步法实现，通过构造闭环系统的Lyapunov函数来获得反馈控制器，并引入Nussbaum函数和虚拟控制律。

2.根据权利要求1所述的一种复杂执行器故障下多冗余机械臂系统协同控制方法，其特征在于，所述步骤S1具体为：

S11、机械臂初始点作为根节点x_init，由此开始生成枝丫；

S12、在机器人的“构型”空间中，生成一个随机点x_rand；

S13、遍历树中的点，找到离x_rand最近的点x_near；

S14、从x_near向x_near方向按一定的步长生长，如果没有碰到障碍物就把生长后的树枝和端点添加到树上，记为x_new，

碰撞条件为：

x₀-r₀-l＜x_new,x＜x₀+r₀+l

y₀-r₀-l＜x_new,y＜y₀+r₀+l

z₀-r₀-l＜x_new,z＜z₀+r₀+l

其中，(x₀,y₀,z₀)为球形障碍物的球心坐标，r₀为球径，l为膨化处理后障碍物的膨胀尺寸；

S15、在新产生的节点x_new附近定义一个半径为r的“区域”，区域中的节点作为x_new父节点的备选

S16、依此计算所有x_init到

的代价与

到x_new的代价和，选取代价和最小的

作为x_new父节点

S17、将剩余的备选节点

的父节点改变为x_new，计算x_init到x_new的代价与x_new到

的代价和；

S18、如果代价和小于之前未更改父节点的代价和，则该

节点的父节点更改为x_new；

S19、重复S12-S18过程，直到到达目标节点x_goal。

3.根据权利要求1所述的一种复杂执行器故障下多冗余机械臂系统协同控制方法，其特征在于，所述步骤S2具体为：

S21、机械臂i的第m个执行器的故障模型为：

其中，μ_i,m表示由执行器故障引起的非零有界比例系数，τ_i,m(t)表示机械臂i的第m个执行器的控制输入，

表示执行器中的有界故障信号，上述故障模型涵盖的执行器故障为：当μ_i,m＝1且

时，系统中的机械臂i的第m个执行器存在噪音干扰；当0＜μ_i,m＜1且

时，系统中的机械臂i的第m个执行器发生部分失效故障；当μ_i,m＜0且

时，系统中的机械臂i的第m个执行器的控制方向发生突变；

S22、采用拉格朗日方程对故障下多冗余机械臂系统动力学进行建模，具体模型如下：

其中，q_i，

分别表示机械臂i的关节位置，关节速度和关节加速度，本设计中的多冗余机械臂系统由i个7自由度冗余机械臂构成，即执行器故障模型中m＝1,...,7，M_i(q_i)∈R^7×7为对称正定惯性矩阵，

为哥氏力和向心力矩阵，G_i(q_i)∈R⁷为重力作用矩阵，

为机械臂i各关节执行器故障后的输出转矩，J_i(q_i)∈R^7×7为反应机器人关节速度到末端执行器速度变换关系的雅可比矩阵，F_ei∈R⁷为机器人末端的输出作用力；

令

则上述多冗余机械臂系统动力学模型表示为：

其中

变换后的模型将在自适应补偿控制算法设计中使用。

4.根据权利要求1所述的一种复杂执行器故障下多冗余机械臂系统协同控制方法，其特征在于，所述步骤S3具体为：

S31、设计多冗余机械臂系统的通信拓扑结构，所述通信拓扑结构用一个加权有向图表示，记为G≡(V,E,A)，其中V＝{v₁,v₂,...,v_N}表示一组节点，节点v_i代表第i个机械臂；

表示边集合，当节点i向节点j发送信息，则边(v_j,v_i)∈E；A＝[a_ij]_N×N表示一个邻接矩阵，其中a_ij表示边(v_i,v_j)的权重，即节点i接收到节点j的信息，当a_ij＝0，则边(v_i,v_j)不存在，参考信号由一个虚拟的线形系统生成，标记为节点v₀，其动力学模型如下：

其中

是领导节点的状态，表示机械臂关节空间的参考信号，q₀为参考关节位置，

为参考关节速度。A∈R^14×14是一个已知的常数矩阵，其形式为一个分块矩阵，即A＝block{0₇,I₇；A₀,A₁}，0₇表示一个7×7的全零矩阵，I₇表示一个7×7的单位矩阵，A₀和A₁表示一个7×7的常数矩阵，并定义受控矩阵为G＝diag{g_i}∈R^N×N；

S32、分布式事件触发状态观测器设计如下：

其中i＝1,...,5，β_i＞0，

表示机械臂i对关节空间的参考信号的估计，

为关节位置估计值，

为关节速度估计值，

表示机械臂i的触发时刻，且

S33、引入事件触发条件，定义了如下新变量：

其中

为机械臂i的邻居误差，代表机械臂i可以接收的信息综合误差，

为经过事件触发后的机械臂i的邻居误差，具体的事件触发机制如下：

其中

θ_i和δ_i是给定的正常数，分别反映

的幅值和趋于零的速率。

5.根据权利要求1所述的一种复杂执行器故障下多冗余机械臂系统协同控制方法，其特征在于，所述步骤S4具体为：

S41、所述控制器的误差矢量定义如下：

其中α_i＝-c₁z_i1是虚拟控制律，且c₁＞0；

S42、设计Nussbaum函数如下：

S43、设计Lyapunov函数：

为信号

估计误差

d_i,m为

的上上确界

为d_i,m的估计值；

S44、设计自适应补偿控制算法：

其中

且ζ_ψ,

c₁,c₂均为正常数。

Images