(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210954097.4 (22)申请日 2022.08.10 (71)申请人 中国地质大 学 （武汉） 地址 430000 湖北省武汉市洪山区鲁磨路 388号 (72)发明人 王诗豪　郑世祺　宛敏红　梁定坤　 黄明燕　 (74)专利代理 机构 武汉知产时代知识产权代理 有限公司 42 238 专利代理师 张磊 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种复杂执行器故障下多冗余机械臂系统 协同控制方法 (57)摘要 本发明提供一种复杂执行器故障下多冗余 机械臂系统协同控制方法， 包括： 机械臂运动约 束算法设计， 所述机械臂运动约束算法采用渐进 最优的RRT*算法， 由碰撞检测、 轨迹规划和正逆 动力学构成； 复杂执行器故障下多冗余机械臂系 统动力学模 型设计； 分布式事件触发状态观测器 设计， 所述 分布式事件触发状态观测器包括通信 拓扑结构、 事件触发机制、 分布式观测器； 自适应 补偿控制器设计， 所述自适应补偿控制器采用反 步法实现， 通过构造闭环系统的L yapunov函数来 获得反馈控制器， 并引入Nussbaum函数和虚拟 控 制律。 本发 明可以有效消除复杂故障对执行器的 影响， 保证设备的正常稳定运行。 权利要求书3页 说明书7页 附图8页 CN 115431263 A 2022.12.06 CN 115431263 A 1.一种复杂执行器故障下多冗余机械臂系统协同控制方法， 其特征在于， 包括以下步 骤： S1、 设计机械臂运动约束算法， 所述机械臂运动约束算法采用渐进最优的RRT*算法， 由 碰撞检测、 轨 迹规划和正逆动力学构成； S2、 设计复杂执 行器故障下多冗余机 械臂系统动力学模型； S3、 设计分布式事件触发状态观测器， 所述分布式事件触发状态观测器包括通信拓扑 结构、 事件触发机制、 分布式观测器； S4、 设计自适应补偿控制器， 所述自适应补偿控制器采用反步法实现， 通过构造闭环系 统的Lyapun ov函数来获得反馈控制器， 并引入Nus sbaum函数和虚拟控制律。 2.根据权利要求1所述的一种复杂执行器故障下多冗余机械臂系统协同控制方法， 其 特征在于， 所述 步骤S1具体为： S11、 机械臂初始点作为 根节点xinit， 由此开始生成枝丫； S12、 在机器人的 “构型”空间中， 生成一个随机点xrand； S13、 遍历树中的点， 找到 离xrand最近的点xnear； S14、 从xnear向xnear方向按一定的步长生长， 如果没有碰到障碍物就把生长后的树枝和 端点添加到树上， 记为xnew， 碰撞条件为： x0‑r0‑l＜xnew,x＜x0+r0+l y0‑r0‑l＜xnew,y＜y0+r0+l z0‑r0‑l＜xnew,z＜z0+r0+l 其中， (x0,y0,z0)为球形障碍物的球心坐标， r0为球径， l为膨化处理后障碍物的膨胀尺 寸； S15、 在新产生的节点xnew附近定义一个半径为r的 “区域”， 区域中的节点作为xnew父节 点的备选 S16、 依此计算所有xinit到 的代价与 到xnew的代价和， 选取代价和最 小的 作为xnew父节点 S17、 将剩余的备选节点 的父节点改变为xnew， 计算xinit到xnew的代价与xnew到 的代价和； S18、 如果代价和小于之前未更改父节点的代价和， 则该 节点的父节点更改为 xnew； S19、 重复S12 ‑S18过程， 直到 到达目标节点xgoal。 3.根据权利要求1所述的一种复杂执行器故障下多冗余机械臂系统协同控制方法， 其 特征在于， 所述 步骤S2具体为： S21、 机械臂i的第m个执 行器的故障模型为： 权　利　要　求　书 1/3 页 2 CN 115431263 A 2其中， μi,m表示由执行器故障引起的非零有界比例系数， τi,m(t)表示机械臂i的第m个执 行器的控制输入， 表示执行器中的有界故障信号， 上述故障模型涵盖的执行器故障 为： 当 μi,m＝1且 时， 系统中 的机械臂i的第m个执行器存在噪音干扰； 当0＜ μi,m＜1 且 时， 系统中的机械臂i的第m个 执行器发生部分失效故障； 当 μi,m＜0且 时， 系统中的机 械臂i的第m个执 行器的控制方向发生 突变； S22、 采用拉格朗日方程对故障下多冗余机 械臂系统动力学进行建模， 具体模型如下： 其中， qi， 分别表示机械臂i的关节位置， 关节速度和关节加速度， 本设计中的多 冗余机械臂系统由i个7自由度冗余机械臂构成， 即执行器故障模型中m＝1, ...,7， Mi(qi)∈ R7×7为对称正定惯性矩阵， 为哥氏力和向心力矩阵， Gi(qi)∈R7为重力作用矩 阵， 为机械臂i各关节执行器故障后的输出转矩， Ji(qi)∈R7×7为反应 机器人关节速度到末端 执行器速度变换关系的雅可比矩阵， Fei∈R7为机器人末端的输出作 用力； 令 则上述多冗余机 械臂系统动力学模型表示 为： 其中 变换后的模型将在自适应 补偿控制算法设计中使用。 4.根据权利要求1所述的一种复杂执行器故障下多冗余机械臂系统协同控制方法， 其 特征在于， 所述 步骤S3具体为： S31、 设计多冗余机械臂系统的通信 拓扑结构， 所述通信拓扑结构用一个加权有向图表 示， 记为G≡(V,E,A)， 其中V＝{v1,v2,...,vN}表示一组节点， 节点vi代表第i个机械臂； 表示边集合， 当节点i向节点j发送信息， 则边(vj,vi)∈E； A＝[aij]N×N表示一个邻 接矩阵， 其 中aij表示边(vi,vj)的权重， 即节点i接收到节点j的信息， 当aij＝0， 则边(vi,vj) 不存在， 参 考信号由一个虚拟的线形系统生成， 标记为节点v0， 其动力学模型如下： 其中 是领导节点的状态， 表示机械臂关节空间的参考信号， q0为参 考关节位置， 为参考关节速度。 A∈R14×14是一个已知 的常数矩阵， 其形式为一个分块矩 阵， 即A＝block{07,I7； A0,A1}， 07表示一个7 ×7的全零矩阵， I7表示一个7 ×7的单位矩阵， A0 和A1表示一个7×7的常数矩阵， 并定义受控矩阵为G＝dia g{gi}∈RN×N； S32、 分布式事 件触发状态 观测器设计如下： 其中i＝1,...,5， βi＞0， 表示机械臂i对关节空间的参考信号的估权　利　要　求　书 2/3 页 3 CN 115431263 A 3