(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111626091.6 (22)申请日 2021.12.28 (71)申请人 航天科工防御技 术研究试验中心 地址 100085 北京市海淀区永定路5 0号 (72)发明人 孙铮　刘净月　王坦　贺洋　 赵慧婷　乔秀铭　徐伟　 (74)专利代理 机构 北京风雅颂专利代理有限公 司 11403 代理人 陈莉 (51)Int.Cl. G06F 30/17(2020.01) G06F 30/20(2020.01) G06F 119/02(2020.01) (54)发明名称 机械臂传感器容 错补偿方法及相关 设备 (57)摘要 本申请提供一种机械臂传感器容错补偿方 法及相关装置， 其中， 所述方法包括： 首先 获取机 械臂每个关节的角度变量的初始值， 计算得到角 度变量的预测值， 根据预测值计算对应传感器的 残差， 根据残差判断对应传感器是否有故障； 之 后获取传感器的测量值， 通过测量值判断传感器 的故障类型； 最后根据传感器的故障类型对传感 器进行容错补偿。 从而根据传感器故障类型的不 同提供不同的容错补偿方案， 能够有效准确的判 断传感器故障的类型和进行故障容 错补偿。 权利要求书2页 说明书13页 附图3页 CN 114372326 A 2022.04.19 CN 114372326 A 1.一种机 械臂传感器容 错补偿方法， 其特 征在于， 包括： 获取机械臂每个关节的角度变量的初始值； 根据所述初始值， 通过无迹卡尔曼滤波算 法对所述角度变量进行 预测， 得到所述角度变量的预测值； 根据所述预测值计算所述角度变量对应的传感器的残差， 响应于所述残差在残差 阈值 范围内， 获取 所述传感器的测量 值； 计算所述测量值的均值和标准差， 根据所述测量值的均值和标准差所在的区间判断所 述传感器的故障类型， 其中， 所述故障类型包括噪声故障、 传感器 漂移和传感器卡死； 响应于确定所述故障类型为可修正范围内的噪声故障， 通过自适应卡尔曼算法计算所 述传感器容错补偿的输出值； 响应于确定所述故障类型为传感器漂移、 传感器卡死或可修 正范围外的噪声故障， 通过平滑过渡的无迹卡尔曼算法计算所述传感器容错补偿的输出 值。 2.根据权利要求1所述的方法， 其特 征在于， 所述方法还 包括： 响应于所述残差在残差阈值范围外， 获取所述传感器的测量值， 将所述测量值作为所 述传感器的输出值。 3.根据权利要求1所述的方法， 其特征在于， 所述根据所述初始值， 通过无迹卡尔曼滤 波算法对所述角度变量进行 预测， 得到所述角度变量的预测值， 包括： 将所述初始值转换为初始值矩阵， 通过方差矩阵计算所述初始值矩阵的协方差矩阵， 通过单位矩阵计算所述初始值矩阵的迭代初值矩阵； 通过无迹变换获取所述迭代初值矩阵对应的Sigma点集， 通过非线性变换计算所述迭 代初值矩阵对应的Sigma点 集对应的预测点 集； 计算所述预测点 集的均值和协方差矩阵； 通过无迹变换获取所述预测点集对应的Sigma点集， 通过非线性变换计算所述预测点 集对应的Sigma点 集对应的观测点 集； 计算所述观测点集的均值， 所述观测点集的协方差矩阵， 所述预测点集和所述观测点 集之间的协方差矩阵； 计算所述观测点集的协方差矩阵与所述预测点集和所述观测点集之间的协方差矩阵 的卡尔曼增益， 根据所述 卡尔曼增益计算所述预测值和所述预测值对应的协方差矩阵。 4.根据权利要求1所述的方法， 其特征在于， 所述根据所述测量值的均值和标准差所在 的区间判断所述传感器的故障类型， 包括： 响应于所述测量值的均值在[0,Cσ 1]之间， 且所述测量值的标准差在[ ‑Cm1,Cm1]之间， 确 定所述传感器正常； 响应于所述测量值的均值在[0,Cσ 1]之间， 且所述测量值的标准差在[ ‑Cm2,‑Cm1]∪[Cm1, Cm2]之间， 确定所述传感器发生漂移故障； 响应于所述测量值的均值在[Cσ 1,Cσ 2]之间， 且所述测量值的标准差在[ ‑Cm1,Cm1]之间， 确定所述传感器发生噪声故障； 响应于所述测量值的均值在[Cσ1,Cσ2]之间， 且所述测量值的标准差在[ ‑Cm2,‑Cm1]∪ [Cm1,Cm2]之间， 确定所述传感器同时发生噪声故障和漂移故障； 其中， Cσ 1表示所述传感器可修正范围的残差标准差的下限值， Cσ 2表示所述传感器可修 正范围的残差标准差的上限值， Cm1表示所述传感器可修正范围残差均值的下限值， Cm2表示权　利　要　求　书 1/2 页 2 CN 114372326 A 2所述传感器可修 正范围残差均值的上限值。 5.根据权利要求1所述的方法， 其特征在于， 所述通过自适应卡尔曼算法计算所述传感 器容错补偿的输出值， 包括： 通过自适应卡尔曼算法 向所述测量值中加入自适应噪声， 将加入所述自适应噪声后的 测量值作为输出值进行输出； 所述自适应噪声满足： 其中， R(t)为时变测量噪声， θfes(t)为t时刻估计值， θfin(t)为时刻t的测量值， T为标准 差的计算周期， kc为可调整的比例系数。 6.根据权利要求1所述的方法， 其特征在于， 所述通过平滑过渡的无迹卡尔曼算法计算 所述传感器容 错补偿的输出值， 包括： 获取所述传感器出现故障前一 时刻的测量值， 根据 所述传感器故障前一 时刻的测量值 和所述预测值 生成与所述预测值平行的中间值； 响应于所述传感器出现故障， 则以从所述中间值到所述预测值平滑过渡的方式输出所 述输出值。 7.根据权利要求6所述的方法， 其特 征在于， 所述平 滑过渡满足： θfout(t)＝θfss(t)·Aes(t)+θfrew(t)·Anew(t) 其中， θfes为预测值， θfnew为中间值， Aes、 Anew为平滑过渡的权重， 满足Aes(t)+Anew(t)＝1； θfout为输出值。 8.一种机 械臂传感器容 错补偿装置， 其特 征在于， 包括： 预测模块， 被配置为获取机械臂每个关节的角度变量的初始值； 根据 所述初始值， 通过 无迹卡尔曼 滤波算法对所述角度变量进行 预测， 得到所述角度变量的预测值； 获取模块， 被配置为根据所述预测值计算所述角度变量对应的传感器的残差， 响应于 所述残差在残差阈值范围内， 获取 所述传感器的测量 值； 判断模块， 被配置为计算所述测量值的均值和标准差， 根据所述测量值的均值和标准 差所在的区间判断所述传感器的故障类型， 其中， 所述 故障类型包括噪声故障、 传感器漂移 和传感器卡死； 补偿模块， 被配置为响应于确定所述故障类型为可修正范围内的噪声故障， 通过自适 应卡尔曼算法计算所述传感器容错补偿的输出值； 响应于确定所述故障类型为传感器漂 移、 传感器卡死或可修正范围外的噪声故障， 通过平滑过渡的无迹卡尔曼算法计算所述传 感器容错补偿的输出值。 9.一种电子设备， 包括存储器、 处理器及存储在存储器上并可在处理器上运行的计算 机程序， 其特征在于， 所述处理器执行所述程序时实现如权利要求1至7任意一项所述的方 法。 10.一种非暂态计算机可读存储介质， 所述非暂态计算机可读存储介质存储计算机指 令， 其特征在于， 所述计算机指令用于使计算机执 行权利要求1至7任一所述方法。权　利　要　求　书 2/2 页 3 CN 114372326 A 3