CN117207190B - 基于视觉与触觉融合的精准抓取机器人系统 - Google Patents

Abstract

本发明公开了基于视觉与触觉融合的精准抓取机器人系统，涉及机器人技术领域，所述系统包括：传感器信息采集部分，包括：视觉传感器和触觉传感器；机器人状态估计部分，配置用于基于机器人的初始状态向量，对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计；融合部分，配置用于基于每个时间步的视觉观测矩阵与状态估计进行融合；机器人状态更新部分，配置用于基于视觉预测和触觉预测，更新机器人在每个时间步的状态估计；抓取指令生成部分，配置用于基于每个时间步的更新状态估计，生成抓取指令。本发明通过整合多模态感知信息和自适应控制策略，提高了抓取精度和稳定性。

Description

基于视觉与触觉融合的精准抓取机器人系统

技术领域

本发明涉及机器人技术领域，特别涉及基于视觉与触觉融合的精准抓取机器人系统。

背景技术

在现代工业和自动化应用中，精准抓取机器人系统的研究和开发变得愈发重要。这些机器人系统被广泛应用于物流、制造业、医疗、仓储和农业等领域，以实现高效的自动化操作。然而，在复杂多变的环境中，机器人要实现精准抓取任务仍然具有挑战性，因为它们需要结合多种感知信息和控制策略，以确保抓取的准确性和鲁棒性。在传统的机器人系统中，通常使用单一的传感器或单一的感知模式来执行抓取任务。例如，常见的机器视觉系统使用摄像头来感知目标物体的位置和姿态，然后执行抓取操作。虽然这些系统在某些应用中表现出色，但它们也存在一些局限性：

传统机器视觉系统在光照不足、物体表面反射性差或物体形状复杂的情况下可能失效。它们对环境条件的依赖性较高，因此在复杂的工作环境中表现不佳。传统系统通常难以实现高精度抓取，特别是在涉及微小物体、脆弱物体或高度精确的任务中。抓取位置和力度的控制不足可能导致抓取失败。某些抓取任务可能需要多种感知模式的融合，例如视觉、触觉和力学信息的联合使用。传统系统通常难以有效地融合多模态感知。在工业生产线或仓储环境中，机器人需要适应不断变化的物体和工作场景。现有技术难以快速适应这些变化，从而限制了机器人的灵活性和效率。

因此如何有效地融合视觉、触觉和其他感知模式的信息，以提高抓取任务的成功率和稳定性，如何在保持高精度的前提下，实现机器人系统对不同物体和环境的适应性，都是重要问题。

发明内容

本发明的目的是提供基于视觉与触觉融合的精准抓取机器人系统，通过整合多模态感知信息和自适应控制策略，提高了抓取精度、稳定性，增强了系统的适应性、鲁棒性和智能性。

为解决上述技术问题，本发明提供基于视觉与触觉融合的精准抓取机器人系统，所述系统包括：传感器信息采集部分，包括：视觉传感器和触觉传感器；所述视觉传感器配置用于采集每个时间步的视觉观测矩阵；所述触觉传感器配置用于采集每个时间步的触觉观测矩阵；机器人状态估计部分，配置用于基于机器人的初始状态向量，对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计；融合部分，配置用于基于每个时间步的视觉观测矩阵与状态估计进行融合，基于每个时间步的触觉观测矩阵与状态估计进行融合，分别得到视觉预测和触觉预测；机器人状态更新部分，配置用于基于视觉预测和触觉预测，更新机器人在每个时间步的状态估计，得到机器人在每个时间步的更新状态估计；抓取指令生成部分，配置用于基于每个时间步的更新状态估计，生成抓取指令，以控制机器人完成抓取动作。

进一步的，设t为时间步；机器人的初始状态向量为x₀；获取初始状态转移矩阵A₀、初始控制输入矩阵B₀和初始噪声w₀；根据初始状态转移矩阵A₀、初始控制输入矩阵B₀初始噪声w₀分别对应生成状态转移矩阵A_t、控制输入矩阵B_t和过程噪声w_t；获取在t时间步的控制输入u_t；机器人状态估计和预测部分，基于机器人的初始状态向量，对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计的过程包括：使用状态转移矩阵A_t、控制输入矩阵B_t和控制输入u_t，以及过程噪声w_t对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计为x_t+1：

x_t+1＝A_tx_t+B_tu_t+w_t；

其中，每个时间步的状态估计x_t为一个向量，包括每个时间步的位置P_t、速度V_t和方向θ_t。

进一步的，设每个时间步的视觉观测矩阵为触觉观测矩阵为/>融合部分，使用如下公式，基于每个时间步的视觉观测矩阵与状态估计进行融合，得到视觉预测：

其中，为视觉预测；/>为视觉噪声；

融合部分，使用如下公式，基于每个时间步的触觉观测矩阵与状态估计进行融合，得到触觉预测：

其中，为触觉预测；/>为触觉噪声。

进一步的，机器人状态更新部分，使用如下公式基于视觉预测和触觉预测，更新机器人在每个时间步的状态估计，得到机器人在每个时间步的更新状态估计：

其中，x_t∣new为机器人在每个时间步的更新状态估计；A_t∣bew为机器人在每个时间步的更新的状态转移矩阵；K_t为增益矩阵。

进一步的，所述增益矩阵使用如下公式计算得到：

其中，为视觉观测矩阵的噪声矩阵；/>为触觉观测矩阵的噪声矩阵。

进一步的，所述机器人在每个时间步的更新的状态转移矩阵使用如下公式计算得到：

其中，I为单位矩阵。

进一步的，抓取指令生成部分，基于每个时间步的更新状态估计，生成抓取指令，以控制机器人完成抓取动作的方法包括：初始化一组粒子每个粒子/>代表更新状态估计x_t∣new对应的每一种抓取指令，计算每一种抓取指令的概率，选择概率最大的抓取指令，控制机器人完成抓取动作；其中N是粒子的数量，也对应抓取指令的类别数量。

进一步的，计算每一种抓取指令的概率的方法包括：

其中，为更新状态估计x_t∣new对应的抓取指令为/>的概率。

进一步的，使用如下公式，根据初始状态转移矩阵A₀生成状态转移矩阵A_t：

其中，P₀为初始位置；V₀为初始速度；θ₀为初始方向；

使用如下公式，根据初始噪声w₀生成过程噪声w_t：

w_t＝Fw₀；

其中，F为噪声增益系数，为设定值。

进一步的，使用如下公式，根据初始控制输入矩阵B₀生成控制输入矩阵B_t：

本发明的基于视觉与触觉融合的精准抓取机器人系统，具有以下有益效果：本发明的机器人系统将视觉和触觉感知信息融合在一起，使得机器人可以更准确地感知和理解目标物体的位置、形状和质地。这种多模态感知的融合使机器人能够执行高精度的抓取操作，即使在光照不足、物体表面反射性差或物体形状复杂的情况下也能表现出色。因此，本发明提高了抓取精度和稳定性，有助于降低抓取任务失败的风险。传统机器人系统在动态环境中的适应性有限，容易受到物体位置变化或外部干扰的影响。本发明的机器人系统采用自适应控制策略，可以实时调整抓取策略，以适应不同物体和环境的变化。这种适应性使机器人能够更好地应对动态环境，提高了系统的鲁棒性。状态估计部分利用多模态感知信息和状态转移矩阵，对机器人在每个时间步的状态进行估计。这包括位置、速度和方向等参数。通过自适应控制策略，机器人能够根据状态估计的结果实时调整抓取策略。例如，如果机器人检测到目标物体的位置发生变化，它可以自动调整抓取点的位置和抓取力度，以适应新的情况。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明实施例提供的基于视觉与触觉融合的精准抓取机器人系统的系统结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1：参考图1，基于视觉与触觉融合的精准抓取机器人系统，所述系统包括：传感器信息采集部分，包括：视觉传感器和触觉传感器；所述视觉传感器配置用于采集每个时间步的视觉观测矩阵；所述触觉传感器配置用于采集每个时间步的触觉观测矩阵；机器人状态估计部分，配置用于基于机器人的初始状态向量，对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计；融合部分，配置用于基于每个时间步的视觉观测矩阵与状态估计进行融合，基于每个时间步的触觉观测矩阵与状态估计进行融合，分别得到视觉预测和触觉预测；机器人状态更新部分，配置用于基于视觉预测和触觉预测，更新机器人在每个时间步的状态估计，得到机器人在每个时间步的更新状态估计；抓取指令生成部分，配置用于基于每个时间步的更新状态估计，生成抓取指令，以控制机器人完成抓取动作。

具体的，视觉传感器通常是摄像头或摄像机，通过捕捉光线的入射和反射来获取图像。这些图像可以是彩色图像、深度图像或红外图像，具体取决于传感器的类型和用途。图像可以以数字形式传输到计算机或嵌入式系统进行处理。触觉传感器通常包括压力传感器、力传感器、力-力矩传感器等，安装在机器人的手爪或触触表面上。这些传感器能够感知物体与机器人之间的接触力、接触面积、力矩等信息。传感器的输出可以是模拟信号或数字信号，取决于传感器类型。

机器人状态估计基于贝叶斯滤波原理，通常使用概率滤波方法来实现。常见的状态估计方法包括卡尔曼滤波、扩展卡尔曼滤波(EKF)、粒子滤波等。这些方法的核心思想是使用传感器测量值来不断更新机器人的状态估计，同时考虑估计的不确定性。通过估计机器人的状态，系统能够理解机器人在环境中的位置、方向、速度等信息。这有助于机器人感知周围环境，包括物体的位置和状态。状态估计提供了机器人当前的位置和速度信息，这对于运动控制非常关键。机器人可以使用这些信息来规划路径、避开障碍物、实施精准的抓取动作等。状态估计不断更新机器人的状态信息，使机器人能够实时响应环境变化。例如，在抓取任务中，如果物体被移动或者机器人被撞击，状态估计可以帮助机器人快速调整动作以适应新的情况。

机器人状态更新部分通常使用概率滤波方法，如卡尔曼滤波、扩展卡尔曼滤波(EKF)或粒子滤波，来更新机器人的状态估计。这些方法基于贝叶斯滤波原理，将新的观测信息融合到先前的状态估计中，以获得更准确的状态估计。在卡尔曼滤波中，机器人的状态估计被建模为高斯分布，包括均值和协方差。通过使用状态转移矩阵和测量矩阵，以及考虑噪声模型，可以预测和校正状态估计。观测更新将测量信息与先前的状态估计融合。对于非线性系统，EKF通过线性化状态空间模型来处理。在每个时间步，EKF使用雅可比矩阵来近似非线性函数，然后应用卡尔曼滤波的更新步骤。粒子滤波不依赖于线性性，它使用一组随机采样的粒子来表示状态估计的概率分布。通过重采样和权重更新，粒子滤波能够逼近真实状态的概率分布，同时处理非线性性和多模态性。通过将最新的视觉和触觉信息融合到状态估计中，状态更新部分有助于不断提高机器人对自身状态的估计准确性。这对于精确的抓取和操作至关重要。状态更新允许机器人实时地调整其动作和决策，以适应环境的变化。例如，当机器人感知到物体位置的微小变化时，状态更新可以引导机器人重新规划抓取策略。机器人状态估计中存在不确定性，状态更新部分有助于管理和减少这种不确定性。它能够在测量噪声和模型不确定性的情况下，提供可靠的状态估计。机器人可以根据最新的状态估计来制定决策，例如选择最佳的抓取策略、路径规划或物体交互方式。这使机器人能够自主地执行任务。

实施例2：设t为时间步；机器人的初始状态向量为x₀；获取初始状态转移矩阵A₀、初始控制输入矩阵B₀和初始噪声w₀；根据初始状态转移矩阵A₀、初始控制输入矩阵B₀初始噪声w₀分别对应生成状态转移矩阵A_t、控制输入矩阵B_t和过程噪声w_t；获取在t时间步的控制输入u_t；机器人状态估计和预测部分，基于机器人的初始状态向量，对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计的过程包括：使用状态转移矩阵A_t、控制输入矩阵B_t和控制输入u_t，以及过程噪声w_t对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计为x_t+1：

x_t+1＝A_tx_t+B_tu_t+w_t；

其中，每个时间步的状态估计x_t为一个向量，包括每个时间步的位置P_t、速度V_t和方向θ_t。

具体的，状态估计和预测的原理基于状态转移矩阵A_t和控制输入矩阵B_t。这些矩阵描述了机器人状态的演化方式和外部控制输入的影响。在每个时间步，机器人的状态根据上一个时间步的状态、控制输入和可能的噪声来进行预测。这可以用数学公式表示为x_t+1＝A_tx_t+B_tu_t+w_t，其中x_t表示在时间步t的状态估计，x_t+1表示在时间步t+1的状态估计。过程噪声w_t考虑了状态估计的不确定性和噪声，使状态估计具有鲁棒性。过程噪声通常被建模为随机变量，通常假设服从某种概率分布，如高斯分布。噪声的存在反映了实际世界中的不确定性。

这个过程的目的是在每个时间步持续更新机器人的状态估计，以反映机器人在环境中的运动和变化。状态转移矩阵和控制输入矩阵描述了状态演化的物理规律，而过程噪声表示了模型的不确定性。控制输入可以来自外部命令，允许对机器人进行控制。通过这个状态估计和预测部分，机器人可以实现实时的自主运动和决策，以应对不同的任务和环境。状态估计和预测允许机器人实时感知和理解自身在环境中的位置、速度和方向等信息。这有助于机器人更好地理解周围环境，包括物体的位置和状态。状态估计和预测支持实时反馈控制，使机器人能够快速响应环境变化。例如，当机器人检测到障碍物或目标位置的变化时，它可以调整自己的运动策略。机器人可以基于最新的状态估计来制定决策，例如选择最佳路径规划、调整速度和方向、决定抓取物体的策略等。这使机器人能够实现自主的任务执行。过程噪声的引入有助于管理状态估计的不确定性。机器人能够在不确定性存在的情况下做出合理的决策，并在不确定性减小时提供更准确的状态估计。

实施例3：设每个时间步的视觉观测矩阵为触觉观测矩阵为/>融合部分，使用如下公式，基于每个时间步的视觉观测矩阵与状态估计进行融合，得到视觉预测：

其中，为视觉预测；/>为视觉噪声；

视觉观测矩阵描述了视觉传感器与机器人状态之间的关系。它通常表示了机器人状态如何映射到视觉观测空间。具体来说，/>可以包括像素到物体坐标的映射、摄像机内外参数等信息。视觉噪声/>表示了视觉观测的不确定性和噪声。视觉传感器在采集图像时会受到噪声的影响，例如图像噪声、光照变化等。机器人状态估计x_t包括了机器人在时间步t的位置、速度、方向等信息。这是基于机器人运动模型和传感器观测信息得出的。视觉预测公式表示为/>其中/>是在时间步t的视觉预测。这个公式将状态估计x_t通过视觉观测矩阵/>映射到视觉观测空间，并考虑了视觉噪声/>

视觉预测允许机器人感知环境中可见的信息，如物体的位置、形状、颜色等。这对于导航、物体检测和环境理解非常重要。通过与视觉观测的融合，机器人可以跟踪目标物体的位置和运动，这在物体追踪和追随任务中很有用。视觉预测可以用于识别环境中的物体或目标。机器人可以比较视觉预测与已知物体的特征，以进行物体识别。视觉预测为机器人的决策制定提供重要信息。例如，在抓取任务中，视觉预测可以告诉机器人何时抓取、如何抓取和抓取哪个物体。视觉预测使机器人能够实时反馈环境变化。如果机器人检测到视觉预测与实际视觉观测不匹配，它可以调整自己的动作和决策以适应新的情况。

融合部分，使用如下公式，基于每个时间步的触觉观测矩阵与状态估计进行融合，得到触觉预测：

其中，为触觉预测；/>为触觉噪声。

触觉观测矩阵

触觉观测矩阵描述了触觉传感器与机器人状态之间的关系。它通常表示了机器人状态如何映射到触觉观测空间。具体来说，/>可以包括触觉传感器的几何安装位置、传感器特性、机械结构等信息。触觉噪声/>表示了触觉观测的不确定性和噪声。触觉传感器在测量物体特性时会受到噪声的影响，如传感器噪声、变形、摩擦等。机器人状态估计x_t包括了机器人在时间步t的位置、速度、方向以及可能的触觉感知结果。这是基于机器人运动模型和传感器观测信息得出的。触觉预测公式表示为/>其中/>是在时间步t的触觉预测。这个公式将状态估计x_t通过触觉观测矩阵/>映射到触觉观测空间，并考虑了触觉噪声/>

触觉预测允许机器人感知物体的特性，如硬度、形状、表面纹理等。这对于机器人抓取、物体识别和交互任务非常重要。机器人可以使用触觉预测来制定抓取策略。例如，通过触觉信息，机器人可以确定如何抓取物体，以最大程度地减少物体滑动或失稳的风险。触觉预测可用于识别物体或目标。机器人可以通过比较触觉预测与已知物体特性的数据库来进行物体识别。触觉预测使机器人能够实时感知物体的特性变化。如果机器人检测到触觉预测与实际触觉观测不匹配，它可以调整自己的动作和决策以适应新的触觉信息。

具体的，视觉观测矩阵是一个关键的组成部分，用于将机器人的状态映射到视觉观测空间。通常，视觉观测矩阵的获取涉及到摄像机标定和相机几何关系的建模。摄像机标定是首要的步骤，用于确定摄像机内外参数，包括焦距、主点坐标、畸变参数等。这通常通过拍摄特定的标定板或标定物体，并使用标定算法来估计这些参数。根据摄像机标定参数，建立相机模型。常见的相机模型包括针孔相机模型、透视投影模型等。这些模型描述了光线如何通过相机的透镜系统和成像传感器进行投影。在机器人控制系统中，通常有一个世界坐标系(例如，机器人的全局坐标系)和一个相机坐标系。视觉观测矩阵的目的是将机器人状态从世界坐标系映射到相机坐标系，因此需要明确定义这两个坐标系之间的转换关系。觉观测矩阵通常由以下部分组成：

内部参数矩阵(IntrinsicMatrix)：包括焦距、主点坐标和畸变参数等，描述了相机内部的几何特性。

外部参数矩阵(ExtrinsicMatrix)：包括相机在世界坐标系中的位置和方向，描述了相机的位置和朝向。

这两个矩阵的组合构成了视觉观测矩阵，用于将世界坐标系中的点映射到相机坐标系中。

一旦获得了视觉观测矩阵，它可以用于将机器人在世界坐标系中的状态映射到相机坐标系中，从而生成视觉观测或预测。这些视觉观测可以用于机器人的感知、导航、物体检测和其他视觉任务。

触觉观测矩阵是将机器人状态映射到触觉传感器观测的关键组成部分。获取触觉观测矩阵通常需要进行触觉传感器的建模和标定。首先，需要建立触觉传感器的模型，包括传感器的类型(如力传感器、力/扭矩传感器、触觉传感器阵列等)、传感器的几何结构(传感器的位置和方向)以及传感器的测量特性(灵敏度、分辨率等)。在机器人控制系统中，通常有一个世界坐标系(例如，机器人的全局坐标系)和一个机器人坐标系。触觉观测矩阵的目的是将机器人状态从世界坐标系映射到触觉传感器坐标系，因此需要明确定义这两个坐标系之间的转换关系。触觉传感器需要进行标定，以确定其在机器人坐标系中的准确位置和方向。这通常包括安装传感器，测量传感器的位置、方向和姿态，并使用标定算法计算触觉传感器的外部参数。

触觉观测矩阵通常由以下部分组成：

外部参数矩阵(ExtrinsicMatrix)：包括触觉传感器在机器人坐标系中的位置和方向。这些参数描述了传感器相对于机器人的姿态。

内部参数矩阵(IntrinsicMatrix)：包括传感器的内部几何参数，如传感器灵敏度、分辨率、畸变等。

这两个矩阵的组合构成了触觉观测矩阵，用于将机器人状态从世界坐标系映射到触觉传感器坐标系。

一旦获得了触觉观测矩阵，它可以用于将机器人在世界坐标系中的状态映射到触觉传感器坐标系中，从而生成触觉观测或预测。这些触觉观测可以用于机器人的感知、决策、控制和物体交互任务。

实施例4：机器人状态更新部分，使用如下公式基于视觉预测和触觉预测，更新机器人在每个时间步的状态估计，得到机器人在每个时间步的更新状态估计：

其中，x_t∣new为机器人在每个时间步的更新状态估计；A_t∣new为机器人在每个时间步的更新的状态转移矩阵；K_t为增益矩阵。

具体的，A_t∣new是机器人在每个时间步的更新状态转移矩阵。它描述了机器人状态从上一个时间步t-1更新到当前时间步t的方式。这个矩阵通常基于机器人的运动模型和控制输入进行计算，用于预测状态的演化。是基于触觉观测和状态估计的触觉预测。它反映了机器人对环境的触觉感知结果，包括物体的特性和状态。触觉预测通常用于更新机器人对物体的触觉信息的信任度。/>是基于视觉观测和状态估计的视觉预测。它反映了机器人对环境的视觉感知结果，包括可见物体的位置、形状和颜色等。视觉预测通常用于更新机器人对可见环境的信任度。u_t是机器人在时间步t的控制输入，用于执行特定的动作或任务。控制输入影响机器人的状态演化和行为。K_t是增益矩阵，用于权衡触觉预测和视觉预测的影响。增益矩阵的选择可以根据任务需求和系统设计来确定，它影响着触觉和视觉信息对状态更新的相对贡献。这个那个将机器人在每个时间步的状态估计x_t-1更新到新的状态估计x_t∣new。这个过程基于触觉和视觉信息，利用状态转移矩阵A_t∣new来预测状态的演化，并使用增益矩阵K_t来平衡触觉和视觉信息的权重。通过融合触觉预测和视觉预测，机器人可以更全面地感知和理解环境。这种多模态融合有助于提高状态估计的准确性，尤其是在复杂环境或有噪声的情况下。更新后的状态估计x_t∣new可以用于机器人的决策制定和控制。机器人可以基于新的状态估计来执行下一步的动作，例如物体抓取、导航、路径规划等。状态更新部分的作用是将机器人的状态估计与多模态感知信息相结合，以获得更准确的状态估计，从而支持机器人在复杂环境中进行决策和操作。这是实现精准抓取和环境交互的关键组成部分。增益矩阵K_t的选择可以根据具体任务的需求进行调整，以平衡不同传感器信息的重要性。

实施例5：所述增益矩阵使用如下公式计算得到：

其中，为视觉观测矩阵的噪声矩阵；/>为触觉观测矩阵的噪声矩阵。

具体的，增益矩阵K_t的作用是在状态更新过程中调整触觉和视觉信息的权重，以融合这两种信息并获得更准确的状态估计。这种融合是基于噪声协方差矩阵的逆矩阵和传感器观测矩阵的乘积，考虑了观测噪声的影响。

如果触觉和视觉信息都很可靠，K_t的值会接近单位矩阵，表示两种信息对状态更新的影响基本相等。如果某一种信息更可靠，K_t的值会调整以更强调可靠信息。

实施例6：所述机器人在每个时间步的更新的状态转移矩阵使用如下公式计算得到：

其中，I为单位矩阵。

具体的，这个公式用以调整状态转移矩阵A_t∣new，以反映多模态感知信息对机器人状态演化的影响。通过减去增益矩阵和观测权重的乘积，可以调整状态演化的速率和方式，使其更适应多模态感知信息的不同贡献。如果多模态感知信息(视觉和触觉观测)对状态估计的影响较大，A_t∣new将受到减弱，从而更多地受到感知信息的影响。相反，如果某种感知信息在某个时间步更为可靠，A_t∣new可能较接近于原始状态转移矩阵A_t，以保持状态演化的稳定性。

首先，计算即视觉观测和触觉观测的权重之和。这个矩阵表示了多模态观测的综合权重。将观测权重矩阵与增益矩阵K_t相乘，得到/>这一步考虑了增益矩阵对观测权重的影响，进一步调整了多模态观测的权重。将上述结果从单位矩阵3·I中减去，得到/> 这个矩阵的作用是将多模态观测权重从状态转移矩阵中减去，以改变状态演化的方式。最后，将上述结果与原始状态转移矩阵A_t相乘，得到机器人在每个时间步t的更新的状态转移矩阵A_t∣new。这个矩阵表示了多模态观测信息对状态演化的影响，其中的权重由增益矩阵和观测权重控制。

实施例7：抓取指令生成部分，基于每个时间步的更新状态估计，生成抓取指令，以控制机器人完成抓取动作的方法包括：初始化一组粒子每个粒子/>代表更新状态估计x_t∣new对应的每一种抓取指令，计算每一种抓取指令的概率，选择概率最大的抓取指令，控制机器人完成抓取动作；其中N是粒子的数量，也对应抓取指令的类别数量。

具体的，粒子滤波是一种用于状态估计的贝叶斯滤波方法，通常用于非线性和非高斯的状态估计问题。它通过在状态空间中采样一组粒子，根据观测信息对这些粒子进行权重更新，最终估计出目标状态的概率分布。这个方法允许系统在每个时间步基于当前状态估计和观测信息来生成最佳的抓取指令。通过采样多个可能的抓取策略(粒子)，系统可以在不确定性的环境中进行决策，并选择最适合当前情况的抓取动作。通过计算抓取指令的概率和粒子权重，系统能够考虑观测信息和任务要求，从而更准确地估计最佳抓取策略，提高了机器人抓取任务的成功率和鲁棒性。

实施例8：计算每一种抓取指令的概率的方法包括：

其中，为更新状态估计x_t∣new对应的抓取指令为/>的概率。

具体的，状态估计差异项(x_t∣new-x_t-1∣new)表示了当前时刻的状态估计x_t∣new与前一时刻的状态估计x_t-1∣new之间的差异。这个差异反映了机器人状态在两个连续时间步之间的演化或变化。如果机器人在两个时间步之间的状态变化较大，这一项的值可能较大，反之亦然。这一项用于考虑机器人状态的演化对抓取指令的可能性的影响。观测信息项：用于考虑机器人当前状态估计与观测信息的一致性。具体来说，它计算了视觉观测与期望的观测值之间的差异，并通过乘以观测噪声协方差矩阵的逆/>来归一化。这个项的目的是确定给定抓取指令的情况下，机器人的观测与期望观测之间的误差有多大，以及这个误差对于抓取指令的可能性有何影响。整个公式通过将状态估计差异项和观测信息项相结合，计算了每种抓取指令的概率/>

具体计算如下：首先，状态估计差异项与观测信息项相减，得到一个综合的影响因子。然后，这个综合的影响因子被归一化为一个概率值，以表示给定抓取指令在当前状态下的相对可能性。这个概率值越高，就意味着该抓取指令越有可能在当前情况下是最佳选择。这个公式的作用是评估每一种抓取指令在当前状态下的适应性和可能性。它综合考虑了状态估计的演化、观测信息的一致性以及观测误差，以生成每种抓取指令的概率分数。这些概率分数可用于选择最佳的抓取策略。当状态估计差异较大时，表明机器人状态发生了较大变化，可能需要调整抓取策略，这会影响概率值。观测信息项考虑了观测的可信度，如果观测信息与期望观测相符，概率值可能较高，反之可能较低。通过将这两个因素结合在一起，公式可以帮助机器人在不同情况下选择最佳的抓取指令，提高抓取任务的成功率和鲁棒性。

符号"∝"(读作"proportionalto")表示"与...成比例"或"与...正比"的关系。在数学和物理中，它通常用来表示两个量之间存在比例关系，即一个量的值随着另一个量的变化而变化，但它们之间可能存在一个比例常数。

实施例9：使用如下公式，根据初始状态转移矩阵A₀生成状态转移矩阵A_t：

其中，P₀为初始位置；V₀为初始速度；θ₀为初始方向；

使用如下公式，根据初始噪声w₀生成过程噪声w_t：

w_t＝Fw₀；

其中，F为噪声增益系数，为设定值。

具体的，初始状态转移矩阵A₀：

首先，给定了初始状态转移矩阵A₀，这个矩阵通常描述了机器人状态如何随时间演化，包括位置、速度和方向等信息。初始位置P₀、初始速度V₀和初始方向θ₀：这些是机器人的初始状态，分别表示初始位置、初始速度和初始方向。

公式表示了状态转移矩阵A_t如何从初始状态转移矩阵A₀和机器人状态的变化率得到。具体来说，它考虑了机器人在位置、速度和方向上的变化率，然后将这些变化率除以时间步t，最终得到了状态转移矩阵A_t。

生成过程噪声w_t的方法：

给定了初始噪声w₀，这通常是一种随机噪声或不确定性，用于模拟机器人状态估计过程中的不确定性或误差。提供了噪声增益系数F，这是一个设定值，用于调整初始噪声w₀的影响。公式w_t＝F*w₀表示了如何生成过程噪声w_t。它简单地将初始噪声w₀与噪声增益系数F相乘，得到了在当前时间步t的过程噪声w_t。

生成状态转移矩阵A_t的方法用于描述机器人状态如何随时间演化。这对于机器人路径规划和运动控制非常重要，因为它定义了机器人在不同时间步的运动方式。生成过程噪声w_t的方法用于引入状态估计过程中的不确定性或误差。这是因为在实际应用中，机器人状态的估计通常受到噪声和不确定性的影响。噪声增益系数F可以调整噪声的强度。

实施例10：使用如下公式，根据初始控制输入矩阵B₀生成控制输入矩阵B_t：

具体的，首先，给定了初始控制输入矩阵B₀，这个矩阵通常描述了机器人的控制策略或动作执行方式。这些矩阵包含了视觉和触觉传感器的观测信息，可能反映了机器人周围环境的特征和状态。公式表示了如何从初始控制输入矩阵B₀和视觉触觉观测矩阵/>中生成控制输入矩阵B_t。这个公式简单地将初始控制输入矩阵B₀与两个观测矩阵的乘积相乘，以生成B_t。生成控制输入矩阵B_t的方法用于调整机器人的控制策略或动作执行方式。这意味着根据当前的视觉和触觉观测信息，可以动态地调整机器人的控制输入，以适应不同的环境或任务需求。视觉触觉观测矩阵/>和/>包含了来自传感器的观测信息，通过与初始控制输入矩阵相乘，可以将这些信息融合到控制输入中，从而影响机器人的行为。

以上对本发明进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (5)

1.基于视觉与触觉融合的精准抓取机器人系统，其特征在于，所述系统包括：传感器信息采集部分，包括：视觉传感器和触觉传感器；所述视觉传感器配置用于采集每个时间步的视觉观测矩阵；所述触觉传感器配置用于采集每个时间步的触觉观测矩阵；机器人状态估计部分，配置用于基于机器人的初始状态向量，对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计；融合部分，配置用于基于每个时间步的视觉观测矩阵与状态估计进行融合，基于每个时间步的触觉观测矩阵与状态估计进行融合，分别得到视觉预测和触觉预测；机器人状态更新部分，配置用于基于视觉预测和触觉预测，更新机器人在每个时间步的状态估计，得到机器人在每个时间步的更新状态估计；抓取指令生成部分，配置用于基于每个时间步的更新状态估计，生成抓取指令，以控制机器人完成抓取动作；

设为时间步；机器人的初始状态向量为/>；获取初始状态转移矩阵/>、初始控制输入矩阵/>和初始噪声/>；根据初始状态转移矩阵/>、初始控制输入矩阵/>初始噪声/>分别对应生成状态转移矩阵/>、控制输入矩阵/>和过程噪声/>；获取在/>时间步的控制输入；机器人状态估计和预测部分，基于机器人的初始状态向量，对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计的过程包括：使用状态转移矩阵/>、控制输入矩阵/>和控制输入/>，以及过程噪声/>对机器人在每个时间步的状态进行估计，得到机器人在每个时间步的状态估计为/>：

；

其中，每个时间步的状态估计为一个向量，包括每个时间步的位置/>、速度/>和方向/>；

设每个时间步的视觉观测矩阵为；触觉观测矩阵为/>；融合部分，使用如下公式，基于每个时间步的视觉观测矩阵与状态估计进行融合，得到视觉预测：

；

其中，为视觉预测；/>为视觉噪声；

融合部分，使用如下公式，基于每个时间步的触觉观测矩阵与状态估计进行融合，得到触觉预测：

；

其中，为触觉预测；/>为触觉噪声；

机器人状态更新部分，使用如下公式基于视觉预测和触觉预测，更新机器人在每个时间步的状态估计，得到机器人在每个时间步的更新状态估计：

；

其中，为机器人在每个时间步的更新状态估计；/>为机器人在每个时间步的更新的状态转移矩阵；/>为增益矩阵；

所述增益矩阵使用如下公式计算得到：

；

其中，为视觉观测矩阵的噪声矩阵；/>为触觉观测矩阵的噪声矩阵；

所述机器人在每个时间步的更新的状态转移矩阵使用如下公式计算得到：

；

其中，为单位矩阵；

触觉观测矩阵由以下部分组成：外部参数矩阵：包括触觉传感器在机器人坐标系中的位置和方向；内部参数矩阵：包括触觉传感器的内部几何参数；外部参数矩阵和内部参数矩阵的组合构成了触觉观测矩阵，用于将机器人状态从世界坐标系映射到触觉传感器坐标系。

2.如权利要求1所述的基于视觉与触觉融合的精准抓取机器人系统，其特征在于，抓取指令生成部分，基于每个时间步的更新状态估计，生成抓取指令，以控制机器人完成抓取动作的方法包括：初始化一组粒子；每个粒子/>，代表更新状态估计/>对应的每一种抓取指令，计算每一种抓取指令的概率，选择概率最大的抓取指令，控制机器人完成抓取动作；其中/>是粒子的数量，也对应抓取指令的类别数量。

3.如权利要求2所述的基于视觉与触觉融合的精准抓取机器人系统，其特征在于，计算每一种抓取指令的概率的方法包括：

；

其中，为更新状态估计/>对应的抓取指令为/>的概率。

4.如权利要求3所述的基于视觉与触觉融合的精准抓取机器人系统，其特征在于，使用如下公式，根据初始状态转移矩阵生成状态转移矩阵/>：

；

其中，为初始位置；/>为初始速度；/>为初始方向；

使用如下公式，根据初始噪声生成过程噪声/>：

；

其中，为噪声增益系数，为设定值。

5.如权利要求4所述的基于视觉与触觉融合的精准抓取机器人系统，其特征在于，使用如下公式，根据初始控制输入矩阵生成控制输入矩阵/>：

。