(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210414943.3 (22)申请日 2022.04.15 (71)申请人 北京邮电大 学 地址 100876 北京市海淀区西土城路10号 (72)发明人 陈钢　王含笑　徐文倩　符颖卓　 李东方　 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种关节部分失效故障空间机械臂运动能 力优化方法 (57)摘要 本发明实施例提供了一种关节部分失效故 障空间机械臂运动能力优化方法， 包括： 依据关 节部分失效故障特征建立基座漂浮的关节部分 失效空间机械臂运动学及动力学模 型， 进而获得 故障机械臂运动学及动力学耦合 关系， 进而基于 关节部分失效故障空间机械臂的运动学和动力 学模型， 构建以末端轨迹跟踪与基座自扰抑制为 控制目标的系统状态空间方程， 进而分别在关节 速度/力矩部分失效情况下， 基于滑模控制方法 设计可克服系统不确定性参数影响的容错控制 器， 实现机械臂末端轨迹跟踪与基座姿态自扰抑 制， 然后基于空间机械臂冗余特性， 分别在关节 速度部分失效与关节力矩部分失效容错控制率 中引入随机变量， 建立机械臂运动能力优化模 型， 并将其与基座扰动情况下的容错控制 系统结 合， 同时实现关节部分失效故障空间机械臂的末 端轨迹跟踪、 基座姿态自扰抑制与运动能力优化。 权利要求书7页 说明书11页 附图7页 CN 114734441 A 2022.07.12 CN 114734441 A 1.一种关节部分失效故障空间机 械臂运动能力优化方法， 其特 征在于， 所述方法包括： (1)依据关节部分失效故障特征建立基座漂浮的关节部分失效空间机械臂运动学及动 力学模型， 进 而获得故障机 械臂运动学及动力学耦合关系； (2)依据所述关节部分失效故障空间机械臂的运动学和动力学模型， 构建以末端轨迹 跟踪与基座自扰抑制为控制目标 的系统状态空间方程， 进而分别在关节速度/力矩部分失 效情况下， 基于滑模控制方法设计可克服系统不确定性参数影响的容错控制器， 实现机械 臂末端轨 迹跟踪与基座姿态自扰抑制； (3)依据所述基座扰动情况下的机械臂容错控制器， 基于空间机械臂冗余特性， 分别在 关节速度部 分失效与关节力矩部分失效容错控制律中引入随机变量， 建立机械臂运动能力 优化模型， 并将其与基座扰动情况下 的容错控制器结合， 同时实现关节部分失效故障空间 机械臂的末端轨 迹跟踪、 基座姿态自扰抑制与运动能力优化。 2.根据权利要求1所述的方法， 其特征在于， 依据关节部分失效故障特征建立基座漂浮 的关节部分失效空间机械臂 运动学及动力学模 型， 进而获得故障机械臂 运动学及动力学耦 合关系， 包括： (1)考虑故障特征建立可同时用于描述关节运动输出部分失效和关节力矩输出部分失 效的故障通用模型： Uk(t)＝Ukc(t)+α(t)( ρk(t)‑1)Ukc(t)+α(t)Ue(t) 其中， Uk(t)为t时刻故障关节k的实际输出， Ukc(t)为t时刻故障关节k的期望输出， 为阶跃函数， Te为k关节故障发生时刻， Tf为任务执行时间， ρk(t) 为t时刻乘性故障关节输出有效因子， ρk(t)∈[0,1]； 当关节k输出的运动发生摄动时， 其输出速度可表示 为： 当关节k输出的力矩发生摄动时， 其输出力矩可表示 为： τk(t)＝ τkc(t)+α(t)( ρk(t)‑1)τkc(t)+α(t)τe(t) (2)结合关节故障通用模型， 建立关节部分失效故障空间机 械臂运动学模型： 其中， vb,wb, 分别为机械臂末端速度、 基座线速度、 基座角速度与关节角速度， n为机械臂自由度数； 为基座速度向末端速度映 射的雅克比矩阵， E∈R3×3为单位矢量， p0e＝pe‑r0为基座到末端的位置矢量， 角标 “×”代表 矢量的反对称 矩阵； Jh,Jf分别为健康关节速度与故障关节速度向末端 速度映射的雅 克比矩 阵， Jh＝[J1,…,Jk‑1,Jk+1,…,Jn]， Jf＝Jk； 为健康关节角速度与故障关节角速度， 权　利　要　求　书 1/7 页 2 CN 114734441 A 2结合关节故障通用模型， 建立关节部分失效故障空间机 械臂动力学模型： 其中， Mb为基座质量矩阵， Mbmh与Mmbh为机械臂、 基座与健康关节耦合的质量矩阵， Mbmf与 Mmbf为机械臂、 基座与故障关节耦合的质量矩阵， Mmhh＝[Mmh1,...,Mmhk‑1,Mmhk+1,…,Mmhn]， Mmhf ＝Mmhk， Mmfh＝[Mmf1,...,Mmfk‑1,Mmfk+1,…,Mmfn]， Mmff＝Mmfk， Mmh与Mmf为机械臂与健康关节和故 障关节耦合的质量矩阵， cb为基座的离心力和哥氏力 矢量， cmh与cmf分别为健康关节与故障 关节的离心力和哥氏力矢量， τmh与 τmf分别为健康关节与故障关节 输出矩阵， 为基座的加 速度， 与 分别为健康关节与故障关节的角加速度； (3)基于基座漂浮空间机械臂线动量和角动量均守恒， 获得健康关节、 故障关节速度与 基座速度之间的映射关系为： 其中， vb与wb为基座的速度及角速度， 与 分别为健康关节与故障关节的角速度， Hbm 分别为基座和机 械臂耦合的动量矩阵； 基于上式及 关节部分失效故障空间机械臂运动学模型获得健康关节、 故障关节速度与 机械臂末端速度之间的映射关系： 其中， Jet为关节速度向末端速度映射的雅克比矩阵； 当空间机械臂故障关节输出力矩部分失效时， 故障关节期望输出速度与实际输出速度 无偏差， 因此其 健康关节、 故障关节与基座、 末端的速度映射关系如上式所示； 当空间机械臂故障关节输出速度部分失效时， 基于关节部分失效故障速度失效模型及 空间机械臂 运动学耦合关系， 获得关节期 望输出速度与基座速度之 间的映射关系以及关节 期望输出速度与末端速度之间的映射关系分别为： 其中， Hb为基座的动量矩阵， 为机械臂末端输出速度， 为故障关节期望输出速度， Hbmc为关节期望速度向基座速度映射的动量矩阵， Hmf为故障关节的动量矩阵， Jet为关节速 度向末端速度映射的雅 克比矩阵， Jev为健康关节速度与故障关节 控制输入速度向末端 速度 映射的雅克比矩阵， Jef为故障关节速度向机 械臂末端速度映射的雅克比矩阵；权　利　要　求　书 2/7 页 3 CN 114734441 A 3