一种基于多信息融合的机械臂末端位置估计方法

1.本发明涉及机械臂位姿测量技术领域，特别是指一种基于多信息融合的机械臂末端位置估计方法。


背景技术：

2.机械臂协助人类解决困难、肮脏、枯燥、危险的工作，被广泛应用于公共服务、工业制造、国家安全、未知领域开拓领域。近年，通过借助外界传感器辅助机械臂实现位置估计的方法应运而生，例如立体视觉、深度视觉、惯性传感器、扫描仪和激光跟踪仪测量末端位置。随着电子技术和图像处理技术的飞速发展，视觉传感器已成为机械臂位置测量领域新的主力军。然而，基于视觉建立的位置模型计算量大、多信息融合存在空域和时域不一致性、融合算法复杂等难题，无法准确地估计末端位置，因此给人们生活带来了巨大的困扰。
3.综上所述，目前机械臂末端位置估计亟需一种准确的、多检测手段融合的方法，实现高精度的机械臂末端位置测量。


技术实现要素：

4.本发明实施例提供了一种基于多信息融合的机械臂末端位置估计方法，能够提高机械臂末端位置估计精度。所述技术方案如下：本发明实施例提供了一种基于多信息融合的机械臂末端位置估计方法，包括：确定靶标；将视觉传感器安装在机械臂的末端，并铺设靶标，通过视觉传感器对铺设的靶标进行位姿检测，以标定视觉传感器与机械臂末端空间位置关系；基于标定的视觉传感器与机械臂末端空间位置关系，得到基于视觉传感器估计的机械臂末端位置；保证视觉传感器采集的图像数据和机械臂反馈数据在空域和时域的一致性的情况下，采用多速率卡尔曼滤波进行数据融合，得到融合后的机械臂末端位置信息。
5.进一步地，所述靶标为由红绿蓝三色构成的立方体的靶标，按照三色不同顺序排列形成6个颜色互异的靶标；同一个靶标6个面的三色排列顺序一致；靶标每个面的三种颜色自形成边长为1.4mm、2.4mm和3.0mm的正方形，3个正方形的中心点相同；靶标具有中心过孔的通孔。
6.进一步地，所述将视觉传感器安装在机械臂的末端，并铺设靶标，通过视觉传感器对铺设的靶标进行位姿检测，以标定视觉传感器与机械臂末端空间位置关系包括：将视觉传感器安装在机械臂的末端，保证光轴方向与延伸杆方向一致，并固联；选择五个靶标，将其中四个靶标铺设为环状，5个中剩下的靶标放置在环状中心；通过调整视觉传感器与机械臂位姿状态，将环状上的四个靶标到中心靶标投影在图像上具有等长的像元数后，通过测量记录视觉传感器与机械臂末端的空间相对位置。
7.进一步地，所述基于标定的视觉传感器与机械臂末端空间位置关系，得到基于视觉传感器估计的机械臂末端位置包括：将6个靶标均匀地铺设在机械臂运动的区域内，通过垂视成像的方式获取靶标在图像中的位置，根据靶标在图像中的位置以及标定的视觉传感器与机械臂末端空间位置关系，并结合视觉传感器自身参数和地面分辨率，得到基于视觉传感器估计的机械臂末端位置。
8.进一步地，得到的基于视觉传感器估计的机械臂末端位置表示为：其中，及都表示基于视觉传感器估计的机械臂末端位置，表示地面分辨率，表示视觉传感器的图像的中心点坐标，表示靶标中心点坐标，表示靶标空间坐标位置，表示焦距，表示像元尺寸，表示视觉传感器与机械臂末端的空间相对位置。
9.进一步地，地面分辨率表示为：其中，和分别表示靶标中三条由长到短的线段对应像元数量。
10.进一步地，所述保证视觉传感器采集的图像数据和机械臂反馈数据在空域和时域的一致性的情况下，采用多速率卡尔曼滤波进行数据融合，得到融合后的机械臂末端位置信息包括：在时域方面，采用嵌入式硬件电路通过外触发控制视觉传感器成像，并获取图像数据，触发信号上升沿和图像帧同步信号的时间间隔；同时，通过外触发控制机械臂控制器，触发信号上升沿和机械臂反馈数据的起始信号的时间间隔，则视觉传感器和机械臂间存在时间差，在触发视觉传感器后，延时时间后触发机械臂控制器，以确保图像数据和机械臂反馈数据的同步；在空域方面，根据标定的视觉传感器与机械臂末端空间位置关系，保证图像数据和机械臂反馈数据的空间一致性；采用多速率卡尔曼滤波的方式，将视觉传感器和机械臂自身传感器各自测量的数据进行融合处理，得到融合后的机械臂末端位置信息。
11.进一步地，得到融合后的机械臂末端位置信息表示为：
其中，下标表示第次视觉传感器成像；表示视觉传感器成像周期内的第次对机械臂位置的采集；表示视觉传感器成像周期与机械臂采集的周期比；表示融合后的机械臂末端位置信息；表示邻帧视觉传感器位置差与同时刻机械臂的位置差的差值；表示机械臂自身估计的三轴末端位置信息；表示基于视觉传感器估计的机械臂末端位置；表示视觉传感器与机械臂末端安装位置差；与分别表示统计特性未知的系统噪声和观测噪声。
12.本发明实施例提供的技术方案带来的有益效果至少包括：本发明实施例中，确定靶标；将视觉传感器安装在机械臂的末端，并铺设靶标，通过视觉传感器对铺设的靶标进行位姿检测，以标定视觉传感器与机械臂末端空间位置关系；基于标定的视觉传感器与机械臂末端空间位置关系，得到基于视觉传感器估计的机械臂末端位置；保证视觉传感器采集的图像数据和机械臂反馈数据在空域和时域的一致性的情况下，采用多速率卡尔曼滤波进行数据融合，得到融合后的机械臂末端位置信息。这样，能够提高机械臂末端位置估计精度。
附图说明
13.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
14.图1为本发明实施例提供的基于多信息融合的机械臂末端位置估计方法的流程示意图；图2为本发明实施例提供的基于多信息融合的机械臂末端位置估计方法的流程框图；图3为本发明实施例提供的靶标示意图；图4为本发明实施例提供的视觉传感器和机械臂末端的空间位置估计示意图；图5为本发明实施例提供的地面分辨率示意图；图6为本发明实施例提供的靶标在图像中映射的示意图；图7为本发明实施例提供的视觉传感器与机械臂在时域上同步关系示意图；图8为本发明实施例提供的多信息融合的机械臂末端位置估计方法的坐标系示意图。
具体实施方式
15.为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。
16.如图1和图2所示，本发明实施例提供了一种基于多信息融合的机械臂末端位置估计方法，包括：s101，确定靶标；本实施例中，根据机械臂控制精度及其运动范围，确定一款色泽各异的、小巧的、独特的靶标；所述靶标为由红绿蓝三色构成的3mm立方体的靶标，按照三色不同顺序排列形成6个颜色互异的靶标，便于鉴别；且同一个靶标6个面的三色排列顺序一致；如图3所示，靶标每个面的三种颜色自形成边长为1.4mm、2.4mm和3.0mm的正方形，3个正方形的中心点相同；靶标具有中心过孔直径为0.2mm的通孔，以便于准确安装固定。
17.s102，将视觉传感器安装在机械臂的末端，并铺设靶标，通过视觉传感器对铺设的靶标进行位姿检测，以标定视觉传感器与机械臂末端空间位置关系；如图4所示，具体可以包括以下步骤：将视觉传感器安装在机械臂的末端，保证光轴方向与延伸杆方向一致，并固联；选择五个靶标，将其中四个靶标铺设为环状，5个中剩下的靶标放置在环状中心；通过调整视觉传感器与机械臂位姿状态，将环状上的四个靶标到中心靶标投影在图像上具有等长的像元数后，通过测量记录视觉传感器与机械臂末端的空间相对位置。
18.本实施例中，通过视觉传感器检测各靶标投影到图像中位置关系，以标定视觉传感器与机械臂末端空间位置关系。
19.s103，基于标定的视觉传感器与机械臂末端空间位置关系，得到基于视觉传感器估计的机械臂末端位置；本实施例中，将6个靶标均匀地铺设在机械臂运动的区域内，通过垂视成像的方式获取靶标在图像中的位置（具体指：靶标中心点位置），根据靶标在图像中的位置以及标定的视觉传感器与机械臂末端空间位置关系，并结合视觉传感器自身参数和地面分辨率，得到基于视觉传感器估计的机械臂末端位置。
20.本实施例中，如图5所示，视觉传感器垂视中，焦距为，像元尺寸。焦面中任意一点和相邻的点，在高度为的情况下，对应的，地面分辨率，表示焦面上一点的横坐标，表示点对应的场景点，表示点对应的场景点。
21.视觉传感器获取靶标图像，根据靶标色泽确定靶标中心点位置，并根据靶标三条不同边长计算的平均值，其中，表示为：
其中，和分别表示靶标中三条由长到短的线段对应像元数量。
22.如图6所示，在已知视觉传感器的图像的中心点坐标，靶标中心点坐标为，靶标空间坐标位置，得到基于视觉传感器估计的机械臂末端位置：其中，表示基于视觉传感器估计的机械臂末端位置。
23.s104，保证视觉传感器采集的图像数据和机械臂反馈数据在空域和时域的一致性的情况下，采用多速率卡尔曼滤波进行数据融合，得到融合后的机械臂末端位置信息，具体可以包括以下步骤：在时域方面，如图7所示，采用嵌入式硬件电路通过外触发控制视觉传感器成像，并获取图像数据，触发信号上升沿和图像帧同步信号的时间间隔；同时，通过外触发控制机械臂控制器，触发信号上升沿和机械臂反馈数据（包括：位置和角度信息）的起始信号的时间间隔，则视觉传感器和机械臂间存在时间差，即图像数据延时于机械臂反馈数据时间为；因此，在触发视觉传感器后，延时时间后触发机械臂控制器，以确保图像数据和机械臂反馈数据的同步；在空域方面，根据标定的视觉传感器与机械臂末端空间位置关系，保证图像数据和机械臂反馈数据的空间一致性；如图8所示，机械臂以基座为坐标系，视觉传感器以地面靶标作为坐标系，靶标和地面基座无轴向距离，只存在水平和方向关系；机械臂末端位置坐标系是建立于机械臂基座坐标系基础上；机械臂和视觉传感器空间上存在的位置距离差；视觉传感器坐标系为和靶标坐标系为，其中，上标表示第个靶标的空间坐标位置，取值1~6；数据融合：考虑到机械臂自身估计的位置信息和视觉传感器估计的位置信息存在更新频率不一致的问题，采用多速率卡尔曼滤波的方式，将视觉传感器和机械臂自身传感器（包括：角度传感器和位置传感器）各自测量的数据进行融合处理，得到融合后的更高精度的机械臂末端位置信息。
24.本实施例中，滤波周期可分为时间更新和量测更新两个过程。当滤波时刻，没有慢速率的视觉传感器的位置测量值，滤波器仅进行机械臂位置信息的更新；当滤波时刻，出现慢速率的视觉位置量测值，滤波器同时进行时间更新和量测更新。
25.本实施例中，得到融合后的机械臂末端位置信息表示为：
其中，下标表示第次视觉传感器成像；表示视觉传感器成像周期内的第次对机械臂位置的采集；表示视觉传感器成像周期与机械臂采集的周期比；表示融合后的机械臂末端位置信息；表示邻帧视觉传感器位置差与同时刻机械臂的位置差的差值；表示机械臂自身估计的三轴末端位置信息；表示基于视觉传感器估计的机械臂末端位置；表示视觉传感器与机械臂末端安装位置差；与分别表示统计特性未知的系统噪声和观测噪声。
26.本实施例中，表示视觉传感器与机械臂末端的空间相对位置邻帧位置差。
27.本实施例中，s101
‑ꢀ
s103构成基于视觉传感器的末端位置估计；s104为基于视觉传感器和机械臂自身传感器多信息融合的末端位置估计。
28.本发明实施例所述的基于多信息融合的机械臂末端位置估计方法，至少包括以下有益效果：1、本发明设计一款由红绿蓝三色构成的、轻小型的、立方体的靶标。靶标由三种不同色泽组成，便于鉴别；靶标采用三种不同尺寸设计，便于视觉传感器采集后像元尺寸量测精度高；靶标具有中心过孔，便于精确安装；通过设计的上述靶标能够降低视觉传感器的辨识、提高视觉传感器测量精度、且靶标便于安装。
29.2、本发明提出通过靶标构建视觉传感器与机械臂的空间位置模型、视觉传感器的末端位置模型，从而降低多传感器安装的误差对系统的影响，同时实现了复杂的视觉传感器末端位置模型，以解决现有的视觉传感器末端位置建模复杂的问题。
30.3、本发明采用多速率卡尔曼滤波实现异步数据在空域和时域的一致性的问题；充分发挥多传感器各自检测的特点，实现多信息数据的互补与融合处理，进而提高机械臂末端位置估计精度，从而解决现有技术存在的多信息融合存在空域和时域不一致性、融合算法复杂、机械臂末端位置估计精度低的问题。
31.以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。