融合多模态数据的机器人位姿伺服控制方法及系统与流程

本发明属于机器人感知与控制，具体涉及融合多模态数据的机器人位姿伺服控制方法及系统。

背景技术：

1、机器人位姿伺服控制用于控制机器人末端执行器或机器人机身按照目标位置和目标姿态运动，是工业装配、精密操作、柔顺接触、表面加工、移动机器人导航和辅助操作设备中的基础技术。

2、机器人位姿伺服控制通常依赖机器人关节编码器、视觉传感器、惯性测量单元或力觉传感器获取机器人运动状态。不同传感器具有不同信息优势。视觉传感器能够提供工作空间中的目标位置、环境结构和机器人末端执行器相对目标的空间关系，但视觉测量容易受到遮挡、照明变化、反光、运动模糊和图像处理延迟影响。惯性测量单元具有采样频率高和短时间动态响应快的特点，但长时间积分容易产生零偏累积和漂移。力觉传感器能够反映机器人末端执行器与外部环境之间的接触状态，但力觉数据在自由运动阶段对机器人绝对位置的约束有限，并容易受到结构振动、工具重力和冲击载荷影响。

3、现有机器人位姿控制方案中，一类方案将视觉位姿测量结果与惯性位姿测量结果通过滤波器进行状态层融合；一类方案利用神经网络从图像中直接估计机器人末端执行器位姿或机器人运动指令；一类方案将视觉信息和力觉信息分别提供给位置控制器和力控制器；还有一类方案利用注意力机制或transformer结构处理多模态特征。不同方案能够改善特定工况下的状态估计或控制性能，但仍存在以下问题。

4、视觉图像、惯性数据和力觉数据的采样频率通常不一致。若仅依据最近时间戳将不同模态数据进行拼接，视觉帧所对应的机器人状态可能已经与当前控制时刻不同，高速运动状态下容易形成时间相位误差。

5、基于神经网络的注意力权重主要从训练数据中学习。当视觉图像发生局部遮挡、惯性测量单元受到振动干扰或力觉传感器发生冲击时，网络注意力得分未必能够直接反映对应传感器的物理可信度。异常模态仍可能获得较高融合权重，并将误差传播到机器人位姿估计结果。

6、部分方案仅向控制器提供确定性的位姿估计值，没有提供位姿估计不确定度。在视觉质量下降或传感器发生漂移时，控制器仍按照固定状态约束和固定接触力约束求解控制量，难以根据感知结果可靠程度调整安全裕量。

7、在线学习方案还可能依赖外部位姿真值。在实际连续运行期间，机器人通常无法持续获得外部光学跟踪设备提供的位姿标签。若无条件利用当前控制误差更新全部神经网络参数，容易将动力学模型误差、执行器迟滞和传感器误差相互混淆，并可能破坏已经形成的闭环稳定状态。

技术实现思路

1、本发明的目的在于提供融合多模态数据的机器人位姿伺服控制方法及系统，使视觉图像、惯性数据和力觉数据能够按照统一控制时刻形成多模态数据窗口，并利用模态质量参数和跨模态物理一致性关系完成特征融合；同时将机器人位姿估计结果及对应不确定度传递至预测控制器，使机器人控制策略能够根据位姿误差、机器人状态和位姿估计可靠程度进行调整。

2、为解决上述技术问题，本发明所采用的技术方案是：

3、融合多模态数据的机器人位姿伺服控制方法，包括：

4、获取机器人视觉图像、惯性数据和力觉数据，按照统一时间基准对视觉图像、惯性数据和力觉数据进行时间对齐、噪声滤波和异常值处理，形成多模态数据窗口；

5、利用卷积神经网络从视觉图像中提取视觉特征，利用循环神经网络从惯性数据中提取惯性特征，利用图神经网络从根据力觉数据构建的力觉交互图中提取力觉图特征；

6、根据视觉图像、惯性数据和力觉数据的数据质量分别确定模态质量参数，根据模态质量参数计算视觉特征、惯性特征和力觉图特征的融合权重，获得多模态融合特征；

7、将多模态融合特征输入位姿估计网络，获得机器人位姿估计结果； 根据机器人位姿估计结果、目标位姿和机器人动力学模型构建预测控制问题，根据位姿误差和机器人状态动态调整位置误差权重、姿态误差权重和控制能量权重，求解控制量；

8、将控制量发送至机器人驱动系统，机器人驱动系统执行控制量后重新获取视觉图像、惯性数据和力觉数据，进入下一控制周期。

9、本发明还包括融合多模态数据的机器人位姿伺服控制系统，用于执行如上所述的融合多模态数据的机器人位姿伺服控制方法，包括：

10、多模态传感数据采集模块，用于获取机器人视觉图像、惯性数据和力觉数据；

11、多模态数据预处理模块，用于按照统一时间基准对视觉图像、惯性数据和力觉数据进行时间对齐、噪声滤波和异常值处理，形成多模态数据窗口；

12、多模态特征提取模块，用于利用卷积神经网络提取视觉特征，利用循环神经网络提取惯性特征，利用图神经网络从力觉交互图中提取力觉图特征；

13、多模态特征融合模块，用于根据视觉图像、惯性数据和力觉数据的数据质量确定模态质量参数，根据模态质量参数计算视觉特征、惯性特征和力觉图特征的融合权重，获得多模态融合特征；

14、位姿估计模块，用于根据多模态融合特征获得机器人位姿估计结果；

15、自适应预测控制模块，用于根据机器人位姿估计结果、目标位姿和机器人动力学模型构建预测控制问题，根据位姿误差和机器人状态调整位置误差权重、姿态误差权重和控制能量权重，并求解控制量；

16、机器人驱动模块，用于执行控制量；

17、闭环状态反馈模块，用于在机器人驱动模块执行控制量后触发多模态传感数据采集模块重新获取视觉图像、惯性数据和力觉数据。

18、与现有技术相比，本发明具有以下有益效果：

19、本发明将视觉图像、惯性数据和力觉数据按照统一时间基准进行对齐，能够减少不同采样频率和数据传输时延造成的时间相位偏差；利用卷积神经网络、循环神经网络和图神经网络分别处理视觉空间信息、惯性时间信息和力觉机械耦合信息，使不同模态保持适合对应数据结构的特征表达方式；通过模态质量参数和跨模态物理一致性残差共同计算融合权重，使融合权重不仅受训练数据驱动，还受到视觉运动、惯性运动和力致位移之间物理关系约束；将机器人位姿协方差传递至预测控制器，能够在位姿估计不确定度增大时收紧机器人状态约束、碰撞距离约束或接触力约束；利用控制一致性残差形成在线校准信号，并通过候选参数验证和参数回滚控制网络参数替换过程，减少连续运行期间缺少外部位姿真值造成的在线学习困难；根据有效模态对应的局部可观测性选择正常控制、受限控制或安全保持控制，避免仅根据注意力权重判断机器人是否具备继续执行六自由度位姿控制的条件。

1.融合多模态数据的机器人位姿伺服控制方法，其特征在于，包括：

2.根据权利要求1所述的融合多模态数据的机器人位姿伺服控制方法，其特征在于，按照统一时间基准形成多模态数据窗口，包括： 以控制周期对应的控制时刻作为数据对齐时刻，选取采集时刻不晚于数据对齐时刻且与数据对齐时刻时间间隔最小的视觉图像； 对相邻视觉图像采集时刻之间的惯性数据进行预积分，并根据从视觉图像采集时刻至数据对齐时刻的惯性数据，将视觉特征外推至数据对齐时刻； 对控制时刻之前的预设力觉时间窗口内的力觉数据进行分段聚合； 根据各数据采集时刻与数据对齐时刻之间的时间偏移量生成时间偏移编码，并对缺失数据、超过预设数据龄期的数据和传感器饱和数据生成有效性掩码。

3.根据权利要求1所述的融合多模态数据的机器人位姿伺服控制方法，其特征在于，噪声滤波采用自适应卡尔曼滤波，自适应卡尔曼滤波根据滑动时间窗口内的预测创新、创新协方差和测量残差更新过程噪声协方差矩阵和观测噪声协方差矩阵，并将过程噪声协方差矩阵和观测噪声协方差矩阵限制在预设正定矩阵范围内。

4.根据权利要求1所述的融合多模态数据的机器人位姿伺服控制方法，其特征在于，构建力觉交互图，包括： 将力觉传感器输出的各力分量、各力矩分量和对应时间采样位置设置为力觉交互图的图节点，图节点特征包括分量值、分量变化率、时间偏移量和传感器有效状态； 根据同一采样时刻不同力分量和力矩分量之间的机械耦合关系建立空间耦合图边，根据相同力分量或力矩分量的相邻采样位置建立时间邻接图边； 利用图神经网络在图节点之间执行消息传递，获得力觉图特征。

5.根据权利要求1所述的融合多模态数据的机器人位姿伺服控制方法，其特征在于，在计算融合权重之前还包括： 根据连续视觉图像确定视觉位姿增量，对惯性数据进行预积分以确定惯性预积分位姿增量，根据力觉数据和机器人末端执行器的柔顺矩阵确定力致弹性位移增量； 将视觉位姿增量、惯性预积分位姿增量和力致弹性位移增量转换至同一机器人坐标系和同一位姿增量空间； 计算视觉位姿增量、惯性预积分位姿增量和力致弹性位移增量之间的跨模态物理一致性残差。

6.根据权利要求5所述的融合多模态数据的机器人位姿伺服控制方法，其特征在于，根据模态质量参数计算融合权重，包括： 根据视觉特征、惯性特征和力觉图特征分别计算初始注意力得分； 根据视觉位姿增量、惯性预积分位姿增量和力致弹性位移增量对应的跨模态物理一致性残差修正初始注意力得分； 将模态质量参数和修正后的注意力得分进行归一化，获得视觉融合权重、惯性融合权重和力觉融合权重。

7.根据权利要求1所述的融合多模态数据的机器人位姿伺服控制方法，其特征在于，位姿估计网络输出机器人位姿均值和机器人位姿协方差矩阵，机器人位姿均值包括机器人三维位置和机器人姿态，机器人位姿协方差矩阵通过位姿估计网络输出的下三角矩阵与下三角矩阵转置的乘积构造。

8.根据权利要求7所述的融合多模态数据的机器人位姿伺服控制方法，其特征在于，根据机器人动力学模型将机器人位姿协方差矩阵沿预测时域进行传播，根据传播后的机器人位姿协方差矩阵和预设置信概率确定约束收紧量，并利用约束收紧量收紧机器人关节约束、机器人末端位姿约束、碰撞距离约束和接触力约束中的至少一种。

9.根据权利要求7所述的融合多模态数据的机器人位姿伺服控制方法，其特征在于，根据位姿误差和机器人状态动态调整位置误差权重、姿态误差权重和控制能量权重，包括：在位置误差或姿态误差增大时提高对应误差权重； 在机器人位姿协方差增大时提高控制能量权重和约束惩罚权重，并降低机器人速度上限和机器人加速度上限； 在位置误差、姿态误差和机器人位姿协方差均低于对应阈值时提高控制能量权重，以减少控制量变化。

10.融合多模态数据的机器人位姿伺服控制系统，用于执行权利要求1-9中任意一项所述的融合多模态数据的机器人位姿伺服控制方法，其特征在于，包括：