机器人力状态估计与可行性修正方法、装置和存储介质与流程

本技术涉及智能机器人控制领域，特别涉及一种机器人力状态估计与可行性修正方法、装置和存储介质。

背景技术：

1、机器人接触作业（例如，曲面恒力跟随、双指抓取、人机协作装配等）是工业与服务机器人领域的核心应用场景之一。在此类作业中，机器人需要通过多模态感知（视觉、力觉、本体觉）实时估计自身与环境的接触状态，包括接触点的位置、法向、间隙、法向/切向接触力以及接触模式（分离、粘着、滑移等），并据此生成力/位混合控制指令，以实现稳定、安全的交互。

2、现有技术中，接触状态的获取主要依赖两类途径：一类是基于物理模型的观测器，另一类是基于数据驱动的多模态融合模型。前者依赖精确的动力学与接触几何建模，在实际环境中因模型误差、标定偏差和传感器噪声而难以保证状态的一致性与可行性；后者虽然能够从大量数据中学习映射关系，但其输出往往缺乏物理约束的显式保证。

3、具体而言，现有技术存在以下不足：1）感知模型或滤波器输出的候选接触状态中可能出现物理上不可能的情况，例如在判定为未接触的同时输出较大的法向接触力，或接触点位于物体表面之外、接触法向与实际几何明显矛盾；2）几何信息与力觉信息之间可能相互冲突，例如视觉深度误差导致工具在几何上穿入刚体数毫米，而力传感器仅显示轻微接触，直接融合此类矛盾信息将产生错误的刚度估计与控制动作；3）摩擦状态缺少可行性校验，估计的切向力可能超出摩擦锥范围却被标记为粘着状态，导致控制器继续增加切向力而非识别滑移；此外，多接触情形下（例如，双指抓取、多点支撑），多个候选接触力可以产生相同的合力，若不引入力矩平衡、压力非负等约束，接触力的分配将不唯一或发生突变；4）当模型误差、标定偏差或传感器故障导致所有约束同时无解时，现有系统往往直接输出上次值或中断控制，缺乏结构化的降级机制与冲突原因指示。

技术实现思路

1、本技术提供一种机器人力状态估计与可行性修正方法、装置和存储介质，通过带权修正候选状态消除物理矛盾，并在无解时安全降级。

2、一方面，本技术提供了一种机器人力状态估计与可行性修正方法，所述方法包括：

3、步骤s1：获取机器人在接触作业过程中的候选接触状态及其可信度信息，以及机器人运动学模型、接触几何模型、材料参数、执行器限值和安全性配置参数；

4、步骤s2：根据所述候选接触状态对应的接触模式，构造包含非穿入与接触互补条件、摩擦锥条件、多接触合力与合力矩平衡条件、接触点运动学一致性条件、执行器限值条件和能量无源性条件的物理约束集合，并根据所述可信度信息为各软约束分配相应的置信权重和最大松弛量，所述接触点运动学一致性条件基于所述机器人运动学模型和所述接触几何模型构造，所述执行器限值条件和所述能量无源性条件为硬约束，所述非穿入与接触互补条件、所述摩擦锥条件、所述多接触合力与合力矩平衡条件以及所述接触点运动学一致性条件为软约束；

5、步骤s3：以所述候选接触状态为初始值，按照所述置信权重，对每一候选模式求解使候选状态到所述物理约束集合的加权投影距离最小的修正状态，输出修正后的接触状态、活动约束标识、硬约束残差、软约束松弛量以及可行性等级，若任一候选模式的硬约束不可同时满足或在预设求解时长内未收敛，则仅保留硬约束并执行安全降级操作，同时输出冲突字段与约束残差；

6、步骤s4：将经过步骤s3验证通过且可行性等级合格的修正状态转换为机器人控制参数并下发执行，采集执行反馈信息，用于更新所述材料参数、所述可信度信息以及所述物理约束集合中的模型参数，并返回步骤s1。

7、另一方面，本技术提供了一种机器人力状态估计与可行性修正装置，所述装置包括：

8、获取模块，用于获取机器人在接触作业过程中的候选接触状态及其可信度信息，以及机器人运动学模型、接触几何模型、材料参数、执行器限值和安全性配置参数；

9、分配模块，用于根据所述候选接触状态对应的接触模式，构造包含非穿入与接触互补条件、摩擦锥条件、多接触合力与合力矩平衡条件、接触点运动学一致性条件、执行器限值条件和能量无源性条件的物理约束集合，并根据所述可信度信息为各软约束分配相应的置信权重和最大松弛量，所述接触点运动学一致性条件基于所述机器人运动学模型和所述接触几何模型构造，所述执行器限值条件和所述能量无源性条件为硬约束，所述非穿入与接触互补条件、所述摩擦锥条件、所述多接触合力与合力矩平衡条件以及所述接触点运动学一致性条件为软约束；

10、求解模块，用于以所述候选接触状态为初始值，按照所述置信权重，对每一候选模式求解使候选状态到所述物理约束集合的加权投影距离最小的修正状态，输出修正后的接触状态、活动约束标识、硬约束残差、软约束松弛量以及可行性等级，若任一候选模式的硬约束不可同时满足或在预设求解时长内未收敛，则仅保留硬约束并执行安全降级操作，同时输出冲突字段与约束残差；

11、转换模块，用于将经验证通过且可行性等级合格的修正状态转换为机器人控制参数并下发执行，采集执行反馈信息，用于更新所述材料参数、所述可信度信息以及所述物理约束集合中的模型参数，并返回获取模块。

12、第三方面，本技术提供了一种电子设备，所述电子设备包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述机器人力状态估计与可行性修正方法。

13、第四方面，本技术提供了一种存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述机器人力状态估计与可行性修正方法。

14、从上述本技术提供的技术方案可知，一方面，通过构造包含非穿入与接触互补条件、摩擦锥条件、多接触合力与合力矩平衡条件、接触点运动学一致性条件、执行器限值条件和能量无源性条件的物理约束集合，并将执行器限值条件和能量无源性条件设定为不可松弛的硬约束，能够消除候选接触状态中的物理矛盾，具体而言，非穿入与接触互补条件保证了间隙与法向力之间的互补关系，杜绝了“未接触却有力”或“穿入却无力”的逻辑矛盾；摩擦锥条件确保了切向力始终位于摩擦力允许范围内，防止控制器在粘着状态下持续增加切向力而忽视滑移风险；多接触合力与合力矩平衡条件结合接触点运动学一致性条件，使得多接触力的分配满足整体受力平衡与运动相容性，避免了力的不唯一或突变，由此，修正后的状态在物理上是自洽且可执行的；另一方面，根据可信度信息为各软约束分配相应的置信权重和最大松弛量，使得修正过程能够自适应地反映各传感器模态的可靠程度。当某传感器字段的协方差较大、数据陈旧或处于饱和/故障状态时，其对应的软约束权重自动降低，意味着该字段在修正中被允许发生较大改变；反之，高可信字段的权重较高，在修正中被尽量保持，这种机制避免了简单裁剪或等权修正可能破坏高可靠观测的问题，使得修正结果在满足物理约束的同时最大限度地保留了原始观测中的可信成分；第三方面，在求解过程中引入可行性等级输出与安全降级机制，当任一候选模式的硬约束不可同时满足或在预设求解时长内未收敛时，系统仅保留硬约束并执行安全降级操作，同时输出冲突字段与约束残差，这使得系统在面对模型误差、标定偏差或传感器故障等异常情况时，不会直接崩溃或输出不可知的状态，而是能够以结构化的方式报告冲突原因并安全退离，从而大幅提升了机器人接触作业的鲁棒性与安全性。综上，本技术的技术方案通过带权修正候选状态消除物理矛盾，并在无解时安全降级。