(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211043455.2 (22)申请日 2022.08.29 (71)申请人 北京空间飞行器总体设计 部 地址 100094 北京市海淀区友谊路104 号 (72)发明人 张晓东　潘冬　梁常春　李剑飞　 王友渔　赵志军　刘鑫　周轶丁　 熊明华　许哲　 (74)专利代理 机构 中国航天科技专利中心 11009 专利代理师 马全亮 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种可重构机械臂操作力突变抑制控制方 法 (57)摘要 本发明涉及一种可重构机械臂操作力突变 抑制控制方法， 依据矩阵摄动理论， 提出的空间 机械臂关节力矩补偿， 实现满足补偿力矩方程以 及含雅克比矩阵的动力学方程， 由此实现广义的 冗余度空间机械臂故障关节力矩的再分配。 同 时， 本发明方法综合最小范数法和最小力矩法优 点， 提出以关节力矩最小化为优化目标得出的关 节加速度逆解方法， 解决最小关节力矩法计算造 成的关节数值的不稳定和关节奇异问题。 权利要求书2页 说明书7页 附图2页 CN 115446832 A 2022.12.09 CN 115446832 A 1.一种可重构机 械臂操作力突变抑制控制方法， 其特 征在于包括： 依据机械臂动力学 方程和矩阵摄动理论， 进行末端操作力突变抑制； 依据关节力矩最优的容错路径规划策略， 进行关节力矩优化， 从而实现空间机械臂在 模型重构和运行参数突变的调整， 实现突变的抑制。 2.根据权利要求1所述的一种可重构机械臂操作力突变抑制控制方法， 其特征在于： 所 述依据机 械臂动力学 方程和矩阵摄动理论， 进行末端操作力突变抑制， 具体包括： 建立冗余度空间机 械臂动力学 方程； 基于所述冗余度空间机械臂动力学方程， 考虑机械臂 的末端受力， 建立考虑末端受力 的机械臂动力学 方程； 确定第k个关节在运动的某时刻t被锁定时， 机 械臂的规划雅各比矩阵kJ； 基于摄动模型思想， 确定故障后的关节速度与末端速度变化关系； 确定故障后的退化机 械臂的末端力变化关系； 确定关节力矩补偿量， 使得故障后的空间机械臂末端力跳变最小， 从而实现末端操作 力突变抑制。 3.根据权利要求2所述的一种可重构机械臂操作力突变抑制控制方法， 其特征在于： 所 述冗余度空间机 械臂动力学 方程， 具体为： 通过拉格朗日法建立冗余度空间机 械臂动力学 方程为： 式中， M(q)是系统的质量矩阵； 是离心和科氏项； G(q)是重力项； τd是提供关节运 动所需要的关节力矩， 其中， q、 分别是关节角位置、 角速度及角加速度向量。 4.根据权利要求3所述的一种可重构机械臂操作力突变抑制控制方法， 其特征在于： 所 述考虑末端受力的机 械臂动力学 方程， 具体为： 其中， JT为雅克比矩阵的转置、 F为末端受力。 5.根据权利要求4所述的一种可重构机械臂操作力突变抑制控制方法， 其特征在于： 机 械臂的规划雅各比矩阵kJ， 具体为： kJ＝[j1,...,jk‑1,jk+1,...,jn] 其中，kJ为k关节故障后的雅各比矩阵。 6.根据权利要求5所述的一种可重构机械臂操作力突变抑制控制方法， 其特征在于： 所 述确定故障后的关节速度与末端速度变化关系， 具体为： 式中， 是故障后空间机械臂关节速度的变化， ΔJ是机械臂关节失效故障后摄动雅克 比矩阵； 为末端速度向量， 是在未进行速度补偿前的退化空间机械臂末端速度跳变； J 是故障前雅可比矩阵。 7.根据权利要求6所述的一种可重构机械臂操作力突变抑制控制方法， 其特征在于： 所 述确定故障后的退化机 械臂的末端力变化关系， 具体为： F+ΔF＝( (JT)++Δ(JT)+)( τ +Δ τ ) 式中， τ是关节输出力矩， Δτ是出现关节失效锁定关节故障后的空间机械臂力矩变化，权　利　要　求　书 1/2 页 2 CN 115446832 A 2JT是雅克比矩阵转置， Δ(JT)+是摄动雅克比矩阵， 而ΔF是未进行关节力矩补偿的空间机械 臂末端力突变。 8.根据权利要求7所述的一种可重构机械臂操作力突变抑制控制方法， 其特征在于： 所 述确定关节力矩补偿量， 具体为： 用ΔU作为关节力矩的补偿量， 有： 式中， 是引入力矩补偿后的机械臂末端力跳变； 当空间机械臂 的第k关节出现关节 故障锁死时有： 其中，k(JT)+是第k关节出现关节故障后雅克比矩阵的伪逆。 化简上式可 得到， 冗余度空间机 械臂末端力跳变最小的力矩补偿为： ΔU＝{Δ(JT)+τ }{k(JT)+}+。 9.根据权利要求8所述的一种可重构机械臂操作力突变抑制控制方法， 其特征在于： 所 述依据关节力矩最优的容 错路径规划策略， 进行关节力矩优化， 包括： (1)在考虑权 重矩阵W的情况 下， 以Z＝ τTW τ 最小化 为优化目标， 确定关节加速度逆解； (2)取权重矩阵W＝D‑1， 并忽略重力项G， 重新确定关节加速度逆解； (3)在计算过程中控制 的数值， 确定改进的关节加速度逆解。 10.根据权利要求9所述的一种可重构机 械臂操作力突变抑制控制方法， 其特 征在于： 在考虑权 重矩阵W的情况 下， 以Z＝ τTW τ 最小化 为优化目标 得出的关节加速度逆解 为： 式中， A＝DWD∈Rn×n， I∈Rn×n为单位矩阵， 为末端加速度、 H为机 械臂质量矩阵、 G为重力向量、 为雅克比矩阵的一阶导数； 取权重矩阵W＝D‑1， 并忽略重力项G， 则关节加速度逆解 为： 式中， 在计算过程中控制 的数值， 得 出的关节加速度逆解的改进计算方法如下： 式中， β 为预先设定的 上限值， 当 时， 采用最小关节力矩法计算， 当 时， 则 采用最小范 数法计算， J+是雅克比矩阵的伪逆。权　利　要　求　书 2/2 页 3 CN 115446832 A 3