CN110116407B

CN110116407B - 柔性机器人位姿测量方法及装置

Info

Publication number: CN110116407B
Application number: CN201910344594.0A
Authority: CN
Inventors: 徐文福; 彭键清; 刘天亮; 梁斌
Original assignee: Shenzhen Graduate School Harbin Institute of Technology
Current assignee: Shenzhen Graduate School Harbin Institute of Technology
Priority date: 2019-04-26
Filing date: 2019-04-26
Publication date: 2021-03-30
Anticipated expiration: 2039-04-26
Also published as: CN110116407A

Abstract

本发明公开了一种柔性机器人位姿测量方法，涉及机器人视觉测量的领域，通过获取初始位姿信息，对测量数据进行扩展卡尔曼滤波，从而得到柔性机器人与充电头的相对位姿信息，并根据相对位姿信息计算机械臂末端的期望速度，进而规划柔性机器人各关节的运动，从而精确对准充电头。通过引入柔性机器人，增大了充电机器人的工作空间并提高了其运动灵活性，并且其多自由度的冗余性非常适合于非结构化的测量环境，并且通过扩展卡尔曼滤波将过去时刻历史累计测量数据对柔性机器人后续运动过程的影响增加到测量结果中，得到补偿后的修正的位姿信息，具有较高的计算精度，无需增加硬件成本，只需一个相机就能获取位姿信息，降低了测量系统的成本，提高了位姿测量的准确性。

Description

柔性机器人位姿测量方法及装置

技术领域

本发明涉及机器人视觉测量的领域，尤其是一种柔性机器人位姿测量方法及装置。

背景技术

如今对车辆、飞机等交通工具进行自主快速充电已成为研究的热点，智能充电机器人通过机械臂的运动识别车辆飞机等设备的充电头，是近年来工业制造领域的一个新兴方向，其主要利用基于视觉的自主识别和位姿测量方法来完成智能自主充电任务。

为了完成智能自主充电任务，首先要解决的技术难点是：充电头的精准识别与精确测量问题。但是由于待充电的测量目标处在非结构化的环境中，且充电头距离机器人比较远，导致充电头的识别与测量有困难。而且以往常采用刚性臂进行充电头测量，其运动空间受限且运动灵活性差，不适合于非结构化的环境，此外，主要通过提高摄像机的光学分辨率来提高测量的精度，虽然使用高分辨率相机和性能更好的硬件系统可以在一定程度上提高位姿测量的准确性，但是这种方式增加了系统成本以及软件操作负担，不符合智能快速充电机器人低成本、高精度测量的要求。

因此需要提出一种能够增大充电机器人的工作空间，以提高机器人运动的灵活性以及位姿测量算法的准确性，在保证测量稳定性的前提下，提高充电对接精度的同时降低位姿测量成本的一种柔性机器人位姿测量方法是很有必要的。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的是为了解决现有的智能充电机器人运动空间受限、运动灵活性差且不适合于非结构化的环境的问题，以及如何在保证测量稳定性的前提下，提高充电对接精度的同时降低位姿测量成本，从而提供一种能够提高精度、节省成本的柔性机器人位姿测量方法和装置。

本发明实施例所采用的技术方案是：

第一方面，本发明实施例提供一种柔性机器人位姿测量方法，包括：

获取充电头图像：通过柔性机器人末端执行器上的视觉测量设备采集充电头图像，所述充电头图像中包括多个圆面；

充电头图像椭圆拟合：根据最小二乘椭圆拟合方法对所述圆面进行椭圆拟合，得到多个椭圆；

获取初始位姿信息：对任意两个所述椭圆进行位姿求解，得到多组参考位姿信息，根据约束条件选择有效参考位姿信息，并根据所述有效参考位姿信息得到所述柔性机器人的初始位姿信息；

获取相对位姿信息：进行多次采集测量，得到多组所述初始位姿信息，进行扩展卡尔曼滤波建模，得到所述柔性机器人末端执行器与所述充电头的相对位姿信息；

获取柔性机器人关节角度数据：获取位姿偏差，根据所述位姿偏差计算所述柔性机器人末端执行器的期望速度，并根据所述期望速度获得所述柔性机器人每个关节的角度数据。

进一步地，还包括对接充电头：

判断所述位姿偏差的值是否大于预设容错阈值；

当所述位姿偏差的值小于所述预设容错阈值时，根据所述每个关节的角度数据，对接所述柔性机器人和所述充电头；

当所述位姿偏差的值大于所述预设容错阈值时，调整所述末端执行器的位姿，重新计算调整后位姿偏差，直至所述调整后位姿偏差的值小于所述预设容错阈值，对接所述柔性机器人和所述充电头。

进一步地，所述位姿偏差包括：位置偏差和姿态偏差，对应的预设容错阈值包括位置容错阈值和姿态容错阈值。

进一步地，所述获取充电头图像包括：获取模板图像，根据模板匹配算法对所述充电头图像进行模板匹配，以得到有效的充电头图像。

进一步地，所述位姿求解包括：根据空间圆的成像原理，选取组成所述椭圆的两个圆面的圆心，分别计算其在视觉测量设备坐标系下的三维位置和法向量以作为参考位姿信息；

所述约束条件为：两个圆面的法向量夹角最小且两个圆心之间的距离最小。

进一步地，所述获取位姿偏差包括：

根据所述相对位姿信息构建充电头参考坐标系；

获取所述充电头和所述末端执行器基于惯性坐标系的旋转矩阵，根据所述旋转矩阵获得所述位置偏差和所述姿态偏差。

第二方面，本发明还提供一种柔性机器人位姿测量装置，包括：

获取充电头图像模块：用于通过柔性机器人末端执行器上的视觉测量设备采集充电头图像，所述充电头图像中包括多个圆面；

充电头图像椭圆拟合模块：用于根根据最小二乘椭圆拟合方法对所述圆面进行椭圆拟合，得到多个椭圆；

获取初始位姿信息模块：对任意两个所述椭圆进行位姿求解，得到多组参考位姿信息，根据约束条件选择有效参考位姿信息，并根据所述有效参考位姿信息得到所述柔性机器人的初始位姿信息；

获取相对位姿信息模块：用于进行多次采集测量，得到多组所述有效参考位姿信息，进行扩展卡尔曼滤波建模，得到所述柔性机器人末端执行器与所述充电头的相对位姿信息；

获取柔性机器人关节角度数据模块：用于获取位姿偏差，根据所述位姿偏差计算柔性机器人末端执行器的期望速度，并根据所述期望速度获得所述柔性机器人每个关节的角度数据；

对接充电头模块：用于判断所述位姿偏差的值与预设容错阈值的大小，并根据判断结果对接所述柔性机器人和所述充电头。

第三方面，本发明还提供一种柔性机器人，包括：柔性机器人本体、视觉测量设备和充电头对接装置，用于根据第一方面任一项所述的一种柔性机器人位姿测量方法进行位姿测量。

第四方面，本发明还提供一种柔性机器人位姿测量设备，包括：

至少一个处理器；以及，与所述至少一个处理器通信连接的存储器；

其中，所述处理器通过调用所述存储器中存储的计算机程序，用于执行如第一方面任一项所述的方法。

第五方面，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行如第一方面任一项所述的方法。

本发明实施例的有益效果是：

本发明实施例通过柔性机器人获取充电头图像，并对充电头图像进行椭圆拟合，进而获取参考位姿信息，并根据约束条件选择有效参考位姿信息，得到初始位姿信息，经过进行多次采集测量，对测量数据进行扩展卡尔曼滤波，从而得到柔性机器人与充电头的相对位姿信息，并根据该相对位姿信息计算机械臂末端的期望速度，进而规划柔性机器人各关节的运动，实现精确对准充电头的效果，通过引入柔性机器人，增大了充电机器人的工作空间并提高了其运动灵活性，并且其多自由度的冗余性非常适合于非结构化的测量环境，并且通过扩展卡尔曼滤波将过去时刻历史累计测量数据对柔性机器人后续运动过程的影响增加到测量结果中，得到补偿后的修正的位姿信息，具有较高的计算精度，无需增加硬件成本，只需要一个相机就能获取位姿信息，降低了测量系统的成本，提高了位姿测量的准确性，能够广泛应用于充电头视觉测量的技术领域。

附图说明

图1是本发明柔性机器人的一具体实施例的示意图；

图2是本发明中柔性机器人位姿测量方法的一具体实施例的实现流程图；

图3是本发明中柔性机器人位姿测量方法的一具体实施例的模板匹配示意图；

图4是本发明中柔性机器人位姿测量方法的一具体实施例的坐标转换示意图；

图5是本发明中柔性机器人位姿测量方法的一具体实施例的相对位姿信息获取示意图；

图6是本发明中柔性机器人位姿测量方法的一具体实施例的具体流程图；

图7是本发明中柔性机器人位姿测量装置的一具体实施例的结构框图。

具体实施方式

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

实施例一：

本实施例提供了一种柔性机器人，如图1所示，为本实施例的柔性机器人示意图，从图中可见，本实施例的柔性机器人包括柔性机器人本体、视觉测量设备3和柔性机器人末端执行器4，柔性机器人本体包括基座1和柔性机械臂2，本实施例中，柔性机器人末端执行器为充电头对接装置，即充电插座，视觉测量设备可选的为单目相机，用于采集目标物5的视觉测量图像，凡是能够实现该功能的视觉测量设备均在本实施例的保护范围之内。

本实施例通过引入柔性机器人，解决现有的智能充电机器人运动空间受限、运动灵活性差且不适合于非结构化的环境的问题，增大了充电机器人的工作空间并提高了其运动灵活性，并且其多自由度的冗余性非常适合于非结构化的测量环境。

本实施例的柔性机器人，用于根据本发明实施例二的一种柔性机器人位姿测量方法进行位姿测量。

实施例二：

本发明实施例二提供一种柔性机器人位姿测量方法，图2为本发明实施例提供的一种柔性机器人位姿测量方法的实现流程图，如图2所示，该方法可以包括以下步骤：

S1：获取充电头图像：通过柔性机器人末端执行器上的视觉测量设备采集充电头图像，如图1中所示目标物，即充电头的外形特点可以看出，其表面具有六个共面圆柱体，因此本实施例中充电头图像中包括多个圆形横截面，即本实施例中的圆面。

采集到的充电头图像后，获取模板图像，再根据模板匹配算法对采集的充电头图像进行模板匹配，以得到有效的充电头图像。进行模板匹配的目的主要是为了提高视觉检测的速度和准确性，其将特征提取限制在检测到的充电端口区域可以有效地消除该区域外部因素的干扰。

S2：充电头图像椭圆拟合：根据最小二乘椭圆拟合方法对圆面进行椭圆拟合，即利用最小二乘椭圆提取算法，计算出的描述椭圆的五个参数分别为长短半轴(a₀,b₀)、中心坐标(x₀,y₀)以及椭圆的偏转角θ₀，本实施例中，有6个圆面，因此可以得到6个椭圆。

S3：获取初始位姿信息：对任意两个椭圆进行位姿求解，得到多组参考位姿信息，根据约束条件选择有效参考位姿信息，并根据有效参考位姿信息得到柔性机器人的初始位姿信息。本实施例中，位姿求解指：根据空间圆的成像原理，选取组成椭圆的两个圆面的圆心，分别计算其在视觉测量设备坐标系下的三维位置和法向量，作为参考位姿信息，约束条件为：两个圆面的法向量夹角最小且两个圆心之间的距离最小。

S4：获取相对位姿信息：进行多次采集测量，得到多组初始位姿信息，进行扩展卡尔曼滤波建模，得到柔性机器人末端执行器与充电头的相对位姿信息，本实施例中进行扩展卡尔曼滤波通过过去时刻累计的测量值误差变化规律，补偿当前状态的测量值，这可以提高目标位姿测量的准确性和可靠性。

S5：获取柔性机器人关节角度数据：获取位姿偏差，根据位姿偏差计算柔性机器人末端执行器的期望速度，并根据期望速度获得柔性机器人每个关节的角度数据。位姿偏差包括：位置偏差和姿态偏差。

本实施例中，获取位姿偏差包括：根据相对位姿信息构建充电头参考坐标系，获取充电头和柔性机器人的末端执行器基于惯性坐标系的旋转矩阵，根据旋转矩阵获得位置偏差和姿态偏差。

S6：对接充电头：判断位姿偏差的值与预设容错阈值的大小，与位姿偏差对应，预设容错阈值包括位置容错阈值和姿态容错阈值。

当位姿偏差的值均小于预设容错阈值时，根据每个关节的角度数据，对接柔性机器人和充电头；当位姿偏差的值均大于预设容错阈值时，调整末端执行器的位姿，根据计算位姿偏差的方法重新计算调整后位姿偏差，直至调整后位姿偏差的值均小于预设容错阈值后，对接柔性机器人和充电头。

步骤S2中的模板匹配的具体过程如下所述。

本实施例中，可选的，首先通过灰度变换和中值滤波算法等图像处理方式，对相机获取的原始图像进行图像增强操作，便于后续处理。为了提高视觉检测的速度和准确性，将特征提取限制在检测到的充电头区域，可以有效地消除该区域外部环境因素的干扰，为了分离充电头部分和环境部分，需要通过模板匹配算法对需要识别的有效目标区域(充电头区域)进行划分。

本实施例的一种实施方式中，创建n×n模板图像，并在模板图像的XY方向上提取其边缘梯度以及2维坐标信息，并通过Sobel梯度算子计算采集的充电头图像的梯度，通过相似性度量将模板图像与采集的充电头图像进行逐点比较，得到充电头图像与模板的相似度最高的位置。进而，根据相似度比较结果，得到匹配后的充电头图像的局部有效像素点，将所有局部有效像素点的集合作为目标搜索区域。

公式表示如下：

^tP_i＝[^tx_i,^ty_i]^T (2)

其中，^tG_i表示模板图像的边缘梯度，

表示模板图像X方向上边缘梯度，

表示模板图像Y方向上边缘梯度，^tP_i表示模板图像中像素点的2维坐标，^sG_u,v表示充电头图像的梯度，

表示充电头图像X方向上边缘梯度，

表示充电头图像Y方向上边缘梯度，H表示充电头图像的X方向上像素数，V表示充电头图像的Y方向上像素数，S_u,v表示点(u，v)的相似度。

如图3所示，为本实施例中模板匹配示意图，图中可以看出，经过模板匹配，从环境中提取到了充电头区域。从充电头的外形特点可以看出，六个共面圆柱体之间的相对距离是已知的，并且圆面的中心由这些圆柱体的交点和表面形成。充电端口记为P1～P6，其中心记为o₁～o₆，充电端口在成像平面的图像坐标系中的成像点记为ABCDEF。

步骤S2中进行充电头图像椭圆拟合的过程为：选取一个圆面，利用最小二乘椭圆提取算法拟合出椭圆，描述椭圆的五个参数分别为长短半轴(a₀,b₀)、中心坐标(x₀,y₀)以及椭圆的偏转角θ₀，并根据拟合得到椭圆计算其标准椭圆方程。

如图4所示，为本实施例的坐标转换示意图，下面根据图中所示详细描述步骤S3中获取初始位姿信息的具体过程，初始位姿信息包括：三维位置矢量和姿态旋转矩阵。

为了便于表达，建立目标对接参考坐标系tg(O_tg-X_tgY_tgZ_tg)，该坐标系的原点为充电头图像的一个边缘顶点，以充电头图像中第一个圆心o₁建立目标参考坐标系ref(O_ref-X_refY_refZ_ref)，根据空间圆的成像原理，计算选取的椭圆对应的目标圆面的圆心在相机坐标系O_C-X_CY_CZ_C下的三维位置和法向量，将其作为参考位姿信息，记为

其在相机坐标系下描述为：

其中，λ₁、λ₂与λ₃是标准椭圆方程系数矩阵的特征值，P_o是与特征值对应特征向量的姿态变换矩阵，R_o是目标圆的半径。

由于法向量有两个解，导致参考位姿信息的解具有二义性，因此选取两个椭圆进行计算会得到四对解，为了去除虚拟解，需要添加适当的约束。本实施例以o₁o₂这两个圆面构成的椭圆为例，获得的参考位姿信息包括：

分别表示第一个椭圆的第一组解、第一个椭圆的第二组解、第二个椭圆的第一组解和第二个椭圆的第二组解。为了获得目标椭圆的法向量，根据约束条件来进行适当的约束，本实施例中，可选的，通过相机在两个目标椭圆的法向量夹角ψ_o最小且两个目标椭圆圆心之间的距离κ_o最小来衡量，约束条件表示为：

其中，

表示两个圆心之间的欧拉距离，根据约束条件约束后得到的有效位姿信息表示为：

即第i个圆心参考位姿信息(即三维位置与法向量)以及第j个圆心参考位姿信息。

假设经过约束条件选择之后，目标参考坐标系ref的三维位置矢量为

其在三个轴的位姿

由下面的公式来确定，即有：

满足条件：

其中，根据图4中示意图可得出：

根据上述公式(11)可以确定

进一步地，可以确定

即确定目标参考坐标系ref相对于相机参考系的位姿。

另外，可以表示出目标参考坐标系ref相对于相机坐标系的齐次变换矩阵，表示如下：

因此，可以推出目标对接坐标系tg相对于相机坐标系的变换关系，表示如下：

其中，^refR_tg表示目标参考坐标系ref到目标对接坐标系tg的旋转矩阵，^reft_tg表示目标参考坐标系ref到目标对接坐标系tg的平移向量。

通过上述变换，可以相应得出，目标对接坐标系上充电头参考点在相机坐标系下的三维位置矢量表示为^cP_tg，以及目标对接坐标系tg相对于相机坐标系的旋转矩阵表示为^cR_tg，即得到初始位姿信息中的三维位置矢量和姿态旋转矩阵。

对于静止的合作目标，当柔性机器人末端执行器靠近充电头时，可以通过过去时刻累计的测量值误差变化规律去补偿当前状态的测量值，即通过扩展卡尔曼滤波提高目标位姿测量的准确性和可靠性。步骤S4中：进行多次采集测量获得柔性机器人末端执行器相对于目标充电头的目标参考坐标系在不同距离下的多组初始位姿信息，记为{X(k)}，将其作为扩展卡尔曼滤波(EKF)模型的输入，输出为更新后的相对位姿信息，记为

如图5所示，为本实施例的相对位姿信息获取示意图，从图中可见初始位姿信息测量值X(k)＝[^cp_tg,^cR_tg]经过扩展卡尔曼滤波之后，卡尔曼滤波器通过估计历史状态来补偿下一状态，提高测量的的准确性，得到更新的测量值

即相对位姿信息。

传统的EKF估计方法是已知的状态转移矩阵和观测矩阵，因此很容易建立状态方程和观测方程来估计预设的状态量。本实施例中，由于不同距离的位姿离散数据没有准确的状态方程，目标的状态转移矩阵只能从历史离散数据中进行估算得到。

本实施例中，假设目标的状态方程为：

X＝[x_t,y_t,z_t,α_x,β_y,γ_z]^T (19)

状态方程表示为：

X(k+1)＝Φ(k+1,k)X(k)+W_k (20)

其中，Φ(k+1,k)是状态变换矩阵，W_k是系统噪声，通常是平均值为零、方差为Q_k的白噪声。

根据实际的初始状态信息测量值，第m个变量X_m的最小值是X_m,min，最大值是X_m,max，将最小间隔[X_m,min,X_m,max]被分成X_m,min,…,X_m,min+i·ΔX_m,…,X_m,max，ΔX_m为设定的间隔，因此，对于任意的初始状态信息测量值X_m,min+i·ΔX_m，可以在下一时刻搜索所有点集得到：

上述X_m,j的概率分布由离散的数据集来确定，其下一时刻的初始状态信息预测值可表示为：

因此，下一刻的初始状态信息预测值可表示为：

X_m(k+1)＝β₀+β₁x_m,i+…+β₁x_m,p+ξ_m,j (23)

根据线性回归方法，Φ_m(k+1,k)、X_m(k)和X_m(k+1)之间的关系可表示为：

X_m(k+1)＝X_m(k)Φ_m(k+1,k)+ξ_m (24)

其中，观测值X_m(k+1)是列向量，ξ_m为包括未观测到的随机分量，回归量的常数观测矩阵X_m(k)为：

根据最小二乘法，状态向量可以通过以下方式获得：

因此，整个系统的状态转换矩阵表示为：

进一步地，观测变量的定义如下：

h(X)＝[x_r,y_r,z_r,α_r,β_r,γ_r]^T (28)

因此，观测方程可写为：

其中，H(k+1)是观测矩阵，V_k是系统的测量噪声，本实施例中，测量噪声是平均值为零、方差为R_k的白噪声。

本实施例中，基于上述EKF的估计方法，校正后的初始位姿信息，即相对位姿信息，表示为：三维位置矢量

与姿态旋转矩阵

本实施例中，柔性机器人末端执行器为充电插座，获得相对位姿信息之后，进行充电头对接充电插座(即柔性机器人的末端执行器)，在对接过程中，相机用于实时测量充电头相对于末端执行器的相对位姿信息，在获得充电头上参考点的坐标

之后，即可根据相对位姿信息建立充电头的目标对接坐标系tg，参考点为充电头上的测量点，即充电头上圆面的圆心。假设充电头目标对接坐标系tg和末端执行器的相机坐标系，相对于大地惯性坐标系的旋转矩阵分别是：R_t和R_e，表示为

和

假设充电头参考点在相机坐标系下的三维位置矢量为^cP_tg，相机坐标系相对于柔性机械臂末端的转换矩阵与平移向量分别为^eR_c,^ep_c。

待对接的充电头相对于末端执行器的位姿偏差包括：位置偏差记为ΔP_et和姿态偏差记为

可分别表示为：

因此，可以根据位姿偏差计算柔性机器人末端执行器的期望速度，并根据期望速度获得柔性机器人每个关节的角度数据，具体计算公式如下所述：

柔性机械臂末端的速度表示为：

其中，K_p是比例参数，K_p是增益矩阵，本实施例中其为单位矩阵。

进一步地，通过速度级逆运动学方程可以确定柔性机器人的关节角速度为：

其中，J_g是雅可比矩阵，

表示雅可比矩阵的逆。

因此，可以通过柔性机器人的关节角速度的数值积分来获得任意时刻的关节角度：

步骤S6中，根据上述位姿偏差的值来对接充电头，具体是：判断位姿偏差的值与预设容错阈值的大小，当位姿偏差的值小于预设容错阈值时，即满足||ΔP_et||>δ_p,||Δψ_et||>δ_θ，其中||ΔP_et||是位置偏差的值，

是姿态偏差的值，δ_p是位置容错阈值，δ_θ是姿态容错阈值，根据公式(34)的关节角度对接柔性机器人的充电插座和充电头，对充电头进行充电。

当位姿偏差的值大于预设容错阈值时，即说明充电插头需要调整位置以更精确的对准充电头，因此调整柔性机器人末端执行器的位姿，并根据本实施例的计算位姿偏差的方法重新计算调整后位姿偏差，直至调整后位姿偏差的值满足小于预设容错阈值的条件，达到对接标准，从而进行柔性机器人和充电头的对接。

如图6所示，为本实施例的一种柔性机器人位姿测量方法具体流程图，从图中可见包括流程：

1)获取初始充电头图像，并对其进行图像增强处理；

2)进行图像模板匹配，提取出充电口区域；

3)进行椭圆检测，并提取椭圆参数；

4)根据任意一组椭圆参数计算充电口的初始位姿信息；

5)利用扩展卡尔曼滤波模型获得相对位姿信息；

6)根据位姿偏差与预设容错阈值的大小关系规划柔性机械臂的运动，包括对接充电头和调整柔性机械臂姿态后对接充电头。

本实施例中，通过引入柔性机器人，增大了充电机器人的工作空间并提高了其运动灵活性，并且其多自由度的冗余性非常适合于非结构化的测量环境，并且通过扩展卡尔曼滤波将过去时刻历史累计测量数据对柔性机器人后续运动过程的影响增加到测量结果中，得到补偿后的修正的位姿信息，具有较高的计算精度，无需增加硬件成本，只需要一个相机就能获取位姿信息，降低了测量系统的成本，提高了位姿测量的准确性。

实施例三：

如图7所示，为本实施例提供的一种柔性机器人位姿测量装置结构框图，包括：

获取充电头图像模块：用于通过柔性机器人末端执行器上的视觉测量设备采集充电头图像，充电头图像中包括多个圆面，该圆面为充电头上圆柱体的横截面。

充电头图像椭圆拟合模块：用于根据最小二乘椭圆拟合方法对圆面进行椭圆拟合，得到多个椭圆。

获取初始位姿信息模块：对任意两个椭圆进行位姿求解，得到多组参考位姿信息，根据约束条件选择有效参考位姿信息，并根据有效参考位姿信息得到柔性机器人的初始位姿信息。

获取相对位姿信息模块：用于进行多次采集测量，得到多组有效参考位姿信息，进行扩展卡尔曼滤波建模，得到柔性机器人末端执行器与充电头的相对位姿信息。

获取柔性机器人关节角度数据模块：用于获取位姿偏差，根据位姿偏差计算柔性机器人末端执行器的期望速度，并根据期望速度获得柔性机器人每个关节的角度数据。

对接充电头模块：用于判断位姿偏差的值与预设容错阈值的大小，并根据判断结果对接柔性机器人和充电头。

另外，本发明还提供一种柔性机器人位姿测量设备，包括：

至少一个处理器，以及与所述至少一个处理器通信连接的存储器；

其中，所述处理器通过调用所述存储器中存储的计算机程序，用于执行如实施例二所述的方法。

另外，本发明还提供一种计算机可读存储介质，计算机可读存储介质存储有计算机可执行指令，其中计算机可执行指令用于使计算机执行如实施例二所述的方法。

本发明通过柔性机器人获取充电头图像，并对充电头图像进行椭圆拟合，进而获取参考位姿信息，并根据约束条件选择有效参考位姿信息，得到初始位姿信息，经过进行多次采集测量，对测量数据进行扩展卡尔曼滤波，从而得到柔性机器人与充电头的相对位姿信息，并根据该相对位姿信息计算机械臂末端的期望速度，进而规划柔性机器人各关节的运动，实现精确对准充电头的效果，能够广泛应用于充电头视觉测量的技术领域。

以上各实施例仅用以说明本发明的技术方案，而非对其限制，尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，其均应涵盖在本发明的权利要求和说明书的范围当中。

Claims

1.一种柔性机器人位姿测量方法，其特征在于，包括：

2.根据权利要求1所述的一种柔性机器人位姿测量方法，其特征在于，还包括对接充电头：

判断所述位姿偏差的值是否大于预设容错阈值；

3.根据权利要求2所述的一种柔性机器人位姿测量方法，其特征在于，所述位姿偏差包括：位置偏差和姿态偏差，对应的预设容错阈值包括位置容错阈值和姿态容错阈值。

4.根据权利要求1所述的一种柔性机器人位姿测量方法，其特征在于，所述获取充电头图像包括：获取模板图像，根据模板匹配算法对所述充电头图像进行模板匹配，以得到有效的充电头图像。

5.根据权利要求1所述的一种柔性机器人位姿测量方法，其特征在于，所述位姿求解包括：根据空间圆的成像原理，选取组成所述椭圆的两个圆面的圆心，分别计算其在视觉测量设备坐标系下的三维位置和法向量以作为参考位姿信息；

6.根据权利要求3所述的一种柔性机器人位姿测量方法，其特征在于，所述获取位姿偏差包括：

根据所述相对位姿信息构建充电头参考坐标系；

7.一种柔性机器人位姿测量装置，其特征在于，包括：

8.一种柔性机器人，其特征在于，包括：柔性机器人本体、视觉测量设备和充电头对接装置，用于根据权利要求1至6任一项所述的一种柔性机器人位姿测量方法进行位姿测量。

9.一种柔性机器人位姿测量设备，其特征在于，包括：

其中，所述处理器通过调用所述存储器中存储的计算机程序，用于执行如权利要求1至6任一项所述的方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行如权利要求1至6任一项所述的方法。