CN108453727A - 基于椭圆特征的机械臂末端位姿误差校正方法及系统 - Google Patents

Abstract

本发明涉及一种基于椭圆特征的机械臂末端位姿误差校正方法，包括获取圆口目标罐的图像，从图像中提取目标椭圆；根据目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系；根据相对位置和轴向关系以及机械臂构形约束，确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量；根据位姿偏差量调整机械臂，直至执行器的位姿偏差量小于预设值。该方法解决了单个圆特征定位存在二义性且无法确定轴向旋转姿态的问题，可为机械臂收集样品放入目标罐中提供精确引导，校正因局部柔性或关节间隙引起的控制误差。本发明还提供一种基于椭圆特征的机械臂末端位姿误差校正系统。

Description

基于椭圆特征的机械臂末端位姿误差校正方法及系统

技术领域

本发明涉及机械臂控制技术领域，尤其涉及一种基于椭圆特征的机械臂末端位姿误差校正方法及系统。

背景技术

随着科学技术的发展，机械臂不但在地面的应用越来越广泛，在地外天体表面着陆探测中也发挥越来越重要的作用，尤其对于地外天体表面的采样操作具有无可替代性。

地外天体表面着陆采样与返回是世界航天大国探索深空和拓展人类认知外星球和太阳系的重要手段。机械臂是地外天体表面着陆采样任务中必不可少的关键设备，能够按照预定的程序自主进行采样任务或者在地面遥操作中心的控制下进行采样作业，可以代替航天员完成各种采样和样品装罐操作，实现地外天体表面的无人化采样。

由于地外天体表面采样的机械臂构形多为类人上肢的多自由度细长结构，在采样操作中存在一定的柔性，同时机械臂关节存在间隙误差，使得机械臂末端的执行器重复定位精度不高。机械臂末端的视觉相机能够获取目标图像，利用图像特征解算目标位置，从而引导机械臂末端的执行器逐步逼近目标位置。然而，现有技术中通常选取图像特征解算出目标与机械臂末端执行器之间的相对位置，根据位置偏差实现机械臂控制，但这种方式要求必须选取能够唯一表示目标的位置状态的图像特征，且该方式仅关注机械臂末端的位置或位姿偏差量，无法实现对机械臂关节角度偏差量的精准控制。

发明内容

本发明所要解决的技术问题是针对现有技术的不足，提供一种基于椭圆特征的机械臂末端位姿误差校正方法及系统。

本发明解决上述技术问题的技术方案如下：一种基于椭圆特征的机械臂末端位姿误差校正方法，包括：

通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像，从所述圆口目标罐的图像中提取目标椭圆；

根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系；

根据所述相对位置和轴向关系以及机械臂构形约束，确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量；

根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂，直至执行器的位姿偏差量小于预设值。

本发明的有益效果是：该方法利用机械臂末端的视觉相机获取目标罐的图像，从图像中获取单个目标椭圆，根据目标椭圆的椭圆特征确定机械臂末端执行器与目标罐的相对位置和轴向关系，进而确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量，根据位姿偏差量设计机械臂操作调整策略。与现有的视觉引导的误差校正方法相比，该方法能够利用机械臂末端视觉相机获取图像中的单个椭圆特征和机械臂构形先验信息计算目标罐的位置和姿态，不仅考虑机械臂末端位姿偏差，还考虑机械臂各关节的偏差量，解决了单个椭圆特征定位存在二义性且无法确定轴向旋转姿态的困难，可为机械臂抓取样品并放入目标罐中提供精确引导，校正因局部柔性或关节间隙引起的控制误差，使遥操作人员准确掌握机械臂末端执行器与目标罐的相对状态和机械臂各关节的当前状态，进而 有效控制机械臂的精准操作，提高操作精度。

在上述技术方案的基础上，本发明还可以做如下改进。

进一步，所述从所述圆口目标罐的图像中提取目标椭圆包括：提取所述图像中的轮廓，通过弧段筛选算法从所述轮廓中剔除无效弧段，选择有效弧段，利用最小二乘算法对每个有效弧段进行椭圆拟合，合并交叠椭圆，剔除无效椭圆，获取目标椭圆。

采用上述进一步方案的有益效果是，通过上述方法可以快速获取图像中的椭圆，且对边界进行多次拟合，合并交叠椭圆，剔除无效椭圆，抑制图像噪声的影响，提高定位精度，准确获取目标椭圆。

进一步，所述根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系包括：

获取所述目标椭圆的椭圆方程描述参数，利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程；

利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面，根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系；

根据手眼标定关系将所述两组第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。

采用上述进一步方案的有益效果是，通过椭锥面标准方程和椭锥面截取等，能够以解析解的方式求得圆口目标罐的相对位置和轴向关系，求解中不引入随机误差，减少了定位解算中的误差源，提高了椭圆定位的整体解算精度。

进一步，所述根据所述相对位置和轴向关系以及机械臂构形约束，确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量包括：

根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且非第一关节轴相互平行的机械臂构形约束，确定机械臂各关节的关节角度和执行器位姿，得到两组执行器位姿及其对应的关节角度；其中，所述末轴垂面为与末端关节轴垂直的执行器所在的平面；

将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量，舍弃两组执行器位姿及其对应关节角度中偏差量大的一组，将偏差量小的一组执行器位姿作为执行器当前位姿，相应地获得当前机械臂各关节的关节角度；将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。

采用上述进一步方案的有益效果是，通过引入机械臂构形约束，能够利用椭圆定位得到的轴向信息解算机械臂的各关节角和执行器位姿，消除椭圆特征定位轴向时存在的姿态不确定性问题，进一步利用机械臂关节角和执行器位姿先验信息消除单一椭圆特征定位的二义性问题，从而得到唯一的相对位置和姿态解。

进一步，所述根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且非第一关节轴相互平行的机械臂构形约束，确定机械臂各关节的关节角度和执行器位姿，得到两组执行器位姿及其对应的关节角度包括：

根据机械臂非第一关节的轴线垂直于第一关节的特性，在末轴垂面上构建简化的机械臂等效模型；将所述第二轴向关系分解为末轴垂面的投影分量和法线分量，建立两组分量与关节角的变换方程，解算第一关节的关节角度和非第一关节的关节角度之和，进而计算执行器相对于机械臂基座的旋转变换矩阵，得到执行器姿态；

利用所述旋转变换矩阵、圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置，确定所述执行器相对于机械臂基座的相对位置，得到执行器位置；其中，所述执行器位姿包括执行器姿态和执行器位置；

根据中间关节的关节角度共面的构形特点，将中间关节的关节角度投影到所述末轴垂面上，构建中间关节的关节角度与机械臂末端坐标位置的等式方程，解算中间关节的关节角度，进而求得末端关节的关节角度。

采用上述进一步方案的有益效果是，结合机械臂的构形特点，将机械臂逆运动学求解中的各关节的关节角度进行解耦处理，将多元联合方程组求解问题化简为多个一元方程的递推求解，简化了求解过程、降低了求解难度，使得仅依靠位置信息和不完全的姿态信息(轴向信息)即能完成所有关节角的逆运动学求解。

进一步，所述通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像时，确定所述视觉相机的光轴方向与圆口目标罐罐口法线方向的夹角，使所述夹角与机械臂执行器的姿态偏差量的差值大于预设值；其中所述执行器的位姿偏差量包括位置偏差量和姿态偏差量。

采用上述进一步方案的有益效果是，通过这一设定能够确保执行器姿态误差量能够被椭圆特征准确反映，不发生因光轴与目标罐罐口法线夹角太小导致执行器姿态误差量难以分辨的情况，确保了椭圆特征定位的唯一性和可靠性。

本发明解决上述技术问题的另一技术方案如下：一种基于椭圆特征的机械臂末端位姿误差校正系统，包括：

图像获取模块，用于通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像；

处理模块，用于从所述圆口目标罐的图像中提取目标椭圆；

用于根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间 的相对位置和轴向关系；

用于根据所述相对位置和轴向关系以及机械臂构形约束，确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量；

控制模块，用于根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂，直至执行器的位姿偏差量小于预设值。

本发明的有益效果是：该系统利用机械臂末端的视觉相机获取目标罐的图像，从图像中获取单个目标椭圆，根据目标椭圆的椭圆特征确定机械臂末端执行器与目标罐的相对位置和轴向关系，进而确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量，根据位姿偏差量设计机械臂操作调整策略。与现有的视觉引导的误差校正相比，该系统能够利用机械臂末端视觉相机获取图像中的单个椭圆特征和机械臂构形先验信息计算目标罐的位置和姿态，不仅考虑机械臂末端位姿偏差，还考虑机械臂各关节的偏差量，解决了单个椭圆特征定位存在二义性且无法确定轴向旋转姿态的困难，可为机械臂抓取样品并放入目标罐中提供精确引导，校正因局部柔性或关节间隙引起的控制误差，使遥操作人员准确掌握机械臂末端执行器与目标罐的相对状态和机械臂各关节的当前状态，进而有效控制机械臂的精准操作，提高操作精度。

在上述技术方案的基础上，本发明还可以做如下改进。

进一步，所述处理模块具体用于：提取所述图像中的轮廓，通过弧段筛选算法从所述轮廓中剔除无效弧段，选择有效弧段，利用最小二乘算法对每个有效弧段进行椭圆拟合，合并交叠椭圆，剔除无效椭圆，获取目标椭圆。

采用上述进一步方案的有益效果是，通过上述方法可以快速获取图像中的椭圆，且对边界进行多次拟合，合并交叠椭圆，剔除无效椭圆，抑制图像噪声的影响，提高定位精度，准确获取目标椭圆。

进一步，所述处理模块具体用于：

获取所述目标椭圆的椭圆方程描述参数，利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程；

利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面，根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系；

根据手眼标定关系将所述第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。

采用上述进一步方案的有益效果是，通过椭锥面标准方程和椭锥面截取等，能够以解析解的方式求得圆口目标罐的相对位置和轴向关系，求解中不引入随机误差，减少了定位解算中的误差源，提高了椭圆定位的整体解算精度。

进一步，所述处理模块具体用于：

根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且非第一关节轴相互平行的机械臂构形约束，确定机械臂各关节的关节角度和执行器位姿，得到两组执行器位姿及其对应的关节角度；其中，所述末轴垂面为与末端关节轴垂直的执行器所在的平面；

将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量，舍弃两组执行器位姿及其对应关节角度中偏差量大的一组，将偏差量小的一组执行器位姿作为执行器当前位姿，相应地获得当前机械臂各关节的关节角度；将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。

采用上述进一步方案的有益效果是，通过引入机械臂构形约束，能够 利用椭圆定位得到的轴向信息解算机械臂的各关节角和执行器位姿，消除椭圆特征定位轴向时存在的姿态不确定性问题，进一步利用机械臂关节角和执行器位姿先验信息消除单一椭圆特征定位的二义性问题，从而得到唯一的相对位置和姿态解。

附图说明

图1是为机械臂、执行器、视觉相机和目标罐结构示意图；

图2是基于椭圆特征的机械臂末端位姿误差校正方法流程图；

图3是对图像中边缘点构成的曲线进行抽样和分割点求取辅助图；

图4是对图像中多个弧段拟合椭圆的合并示意图；

图5是空间目标圆向像平面的投影示意图；

图6是机械臂构形、视觉相机、圆口目标罐及尺寸标注图；

图7是简化的等效机械臂构形、视觉相机、圆口目标罐及尺寸标注图；

图8是机械臂基座及各关节中心、相机、圆口目标罐的坐标系建立图；

图9是机械臂的俯视投影及其坐标轴描述图；

图10是机械臂的侧视投影及其坐标轴描述图；

图11是基于椭圆特征的机械臂末端位姿误差校正系统示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实施例只用于解释本发明，并非用于限定本发明的范围。

图1为机械臂、执行器、视觉相机和目标罐结构示意图。其中执行器固定在机械臂末端，视觉相机固定在执行器上，目标罐固定于视觉相机的拍摄范围内。

图2给出了本发明实施例提供的一种基于椭圆特征的机械臂末端位姿误 差校正方法的示意性流程图。如图2所示，该方法包括：

S1，通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像，从所述圆口目标罐的图像中提取目标椭圆；

S2，根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系；

S3，根据所述相对位置和轴向关系以及机械臂构形约束，确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量；

S4，根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂，直至执行器的位姿偏差量小于预设值。

上述实施例中，该方法利用机械臂末端的视觉相机获取目标罐的图像，从图像中获取单个目标椭圆，根据目标椭圆的椭圆特征确定机械臂末端执行器与目标罐的相对位置和轴向关系，进而确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量，根据位姿偏差量设计机械臂操作调整策略。与现有的视觉引导的误差校正方法相比，该方法能够利用机械臂末端视觉相机获取图像中的单个椭圆特征和机械臂构形先验信息计算目标罐的位置和姿态，不仅考虑机械臂末端位姿偏差，还考虑机械臂各关节的偏差量，解决了单个椭圆特征定位存在二义性且无法确定轴向旋转姿态的困难，可为机械臂抓取样品并放入目标罐中提供精确引导，校正因局部柔性或关节间隙引起的控制误差，使遥操作人员准确掌握机械臂末端执行器与目标罐的相对状态和机械臂各关节的当前状态，进而有效控制机械臂的精准操作，提高操作精度。

现有技术中利用单个椭圆特征解算目标罐的位姿，会得到两个对称的解，这称为椭圆特征定位的二义性。由于圆形目标罐的对称特征，相机从不同角度看，能够得到相同形状的椭圆，这使得在根据椭圆特征定位时，仅能解算轴向法线，不能完全确定圆形目标罐与相机的姿态关系。

该实施例中，利用单个椭圆特征和机械臂构形信息能够计算得到目标罐唯一的位置和姿态，主要是由于在利用椭圆特征定位解算中，考虑了机械臂的构形约束，使得最终的位置和姿态解唯一。

在基于视觉特征的操作定位中，图像特征的选择和确定模型直接影响最终定位的精度，合理选择目标特征和构建确定模型是实现视觉引导精确操作的关键，对实现地外天体表面采样具有重要意义。

可选地，作为本发明另外一个实施例，S1，通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像，从所述圆口目标罐的图像中提取目标椭圆包括：通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像，提取所述图像中的轮廓，通过弧段筛选算法从所述轮廓中剔除无效弧段，选择有效弧段，利用最小二乘算法对每个有效弧段进行椭圆拟合，合并交叠椭圆、剔除无效椭圆，获取目标椭圆。

该实施例中，通过上述方法可以快速获取图像中的椭圆，且对边界进行多次拟合，合并交叠椭圆，剔除无效椭圆，抑制图像噪声的影响，提高定位精度，准确获取目标椭圆。

弧段筛选算法对图像中提取出的边缘点进行编组和筛选，舍去无效弧段。具体包括如下三个步骤：第一步是边缘跟踪连接与细化，第二步是对连接和细化的曲线进行抽样和分割，第三步是对无效弧段进行剔除。结合图3对弧段筛选算法介绍如下：

第一步是边缘跟踪连接与细化：在圆口目标罐的图像中搜出一个边缘点，再以该边缘点为起点在周围8邻域进行边缘点搜寻，加入到边缘序列点中构建曲线。如果周围8邻域搜得的边缘点数量大于2，判断边缘是否有分叉，如果边缘有分叉，则将分叉边缘定义为一个新的边缘曲线，如果边缘无分叉则删除多余的边缘点。

第二步是对连接和细化的曲线进行抽样和分割：首先对离散点组成的弧 段以固定的弧长ΔS(可令ΔS＝3)进行抽样，得到新的离散点弧段，如图3所示。然后对离散点弧段进行分割点搜寻分割弧段，方法如下：如果相邻两点连线的角度差大于给定阈值δ_c即|θ₁-θ₂|＞δ_c，或者相邻两点连线的斜率单调变化即(k₂-k₁)(k₃-k₂)＜0，表达为坐标P_i(x_i,y_i)的形式为(Δx₁₂Δy₂₃-Δy₁₂Δx₂₃)(Δx₂₃Δy₃₄-Δy₂₃Δx₃₄)＜0，其中，Δx_ij＝x_j-x_i，Δy_ij＝y_j-y_i，则在P₃位置对弧段进行分割。其中P_i(x_i,y_i)表示离散点的坐标，θ₁表示直线P₁P₂与直线P₂P₃的夹角，θ₂表示直线P₂P₃与直线P₃P₄的夹角，k₁,k₂,k₃分别表示直线P₁P₂、直线P₂P₃和直线P₃P₄的斜率。

第三步是对无效弧段进行剔除：计算每个弧段长度L_i和曲率变化量κ_i，即κ_i＝Δθ_i/L_i。剔除长度小于L_min的弧段和近似直线的弧段，即L_i＜L_min或者κ_i＜ε(ε为一微小量)的弧段。

基于弧段筛选算法得到多个有效弧段，对每个弧段采用最小二乘算法拟合椭圆，求取椭圆方程描述参数的方法描述如下。椭圆方程可通过带约束的二次曲线进行描述，其一般形式为：

s.t.表示优化问题的约束(subjected to)，优化问题的数学表达方式。令Θ＝[AB C D E F]，u＝[x² 2xy y² 2x 2y 1]，上述公式中的系数Θ可以根据比例因子不同产生无数组解，均不影响u的取值。为了实现Θ解的唯一性，可指定一个尺度因子，如F＝1或者||Θ||＝1。拟合求解过程给出如下。

令表示图像上的第i个像素点，则根据公式(1-1)可得优化问题：

其中，表示以Θ为变量求解最小值的优化问题，表示求取以Θ为变量的函数||UΘ||²最小值的优化问题，表示所有像素点集，H是由AC-B²＞0得出的约束矩阵，定义为：

对公式(1-3)中的优化问题进行求解，通过引入拉格朗日因子λ，将带约束的优化问题转化为无约束问题，并对二次目标函数进行求导运算可得：

令S＝U^TU，考虑U、S和H的特殊性，我们进行矩阵分块得：

根据上述分块关系，可重新构建矩阵方程：

S₁Θ₁+S₂Θ₂＝λH₁Θ₁， (1-5)

由公式(1-6)可知，当S₃为非奇异矩阵时，Θ₂可表示为Θ₁的线性变换，即将其带入公式(1-5)可得：

由于H₁为非奇异矩阵，公式(1-7)可写成：

公式(1-4)中的等式约束重写为：

Θ₁H₁Θ₁＝1. (1-9)

令则椭圆拟合求解问题转化为求解MΘ₁＝λΘ₁的问题，即求取矩阵M的特征值的问题。考虑到Θ₁H₁Θ₁＝1对特征值非负性的约束，取M的非负特征值对应的特征向量作为Θ₁的解。根据求得Θ₂，从而得到椭圆曲线一般方程的系数Θ，即得到二次曲线椭圆方程的描述参数A,B,C,D,E,F。

为了抑制图像噪声的影响，提高定位精度，可对边界进行多次拟合。即第一次拟合后，将每个边界点代入上式，计算残差。然后将残差较大的一部分点剔除，再对剩余的点进行二次椭圆拟合。该过程可以重复若干次，直到均方差小于某一阈值为止。

基于上述的二次曲线椭圆方程的描述参数，可以求解椭圆方程的标准形式为：

椭圆方程的标准形式包含椭圆中心(x₀,y₀)、椭圆方向角θ和长短半轴a、b共5个参数。利用二次曲线椭圆方程的描述参数A,B,C,D,E,F计算出这5个参数的计算公式如下，其中尺度因子F＝1：

其中，(x₀,y₀)为拟合椭圆中心坐标，θ为长半轴与图像坐标系x轴夹角。根据式(1-1)、(1-12)进行变换得：

根据式(1-13)得到的参数可以求出椭圆的长、短半轴a和b如下。

在得到(x₀,y₀)、θ和长短半轴a、b后，可利用多个有效弧段各自拟合出的椭圆的相似关系，合并同一椭圆的弧段为一弧段组。合并方法结合图4介绍如下。为每个有效弧段定义旋转坐标系，坐标系原点定义为拟合椭圆的中心，坐标系起始轴定义为与图像x轴平行方向，定义逆时针旋转方向为正，顺时针旋转方向为负，定义旋转角度范围为[-2π,2π]。图4中的三个弧段i,j,k对应的旋转角度范围分别为Γ_i[α_i1,α_i2]、Γ_j[α_j1,α_j2]、Γ_k[α_k1,α_k2]，对应的拟合椭圆参数分别为Ω_i(x_i,y_i,a_i,b_i,θ_i)、Ω_j(x_j,y_j,a_j,b_j,θ_j)、Ω_k(x_k,y_k,a_k,b_k,θ_k)。如果满足||Ω_H-Ω_G||₂≤ξ_Ω且其中H,G∈{i,j,k}，ξ_Ω为给定的阈值，则将弧段H和弧段G合并为同一组。

对弧段组内的多个弧段应用上述的最小二乘拟合算法获得新的椭圆，选择弧段组内弧段覆盖角度最大的椭圆作为最终的目标罐口椭圆。

上述过程中假设图像中背景较为简单，仅存在目标罐口为圆形特征，不存在其他的形状以及尺寸近似的干扰椭圆，则能够选出唯一的目标罐口椭圆。对于复杂背景下存在干扰椭圆的情况，则可以通过引入机械臂末端位姿的近似位置作为先验信息，根据透视投影原理计算目标椭圆的大致位置、方向与尺寸，筛选掉偏离较大的椭圆，保留唯一的目标罐口椭圆。

可选地，作为本发明另一个实施例，S2，根据所述目标椭圆的椭圆特征 确定圆口目标罐罐口与执行器之间的相对位置和轴向关系包括：

获取所述目标椭圆的椭圆方程描述参数，利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程；

利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面，根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系；

根据手眼标定关系将所述第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。

该实施例中，通过椭锥面标准方程和椭锥面截取等，能够以解析解的方式求得圆口目标罐的位置和轴向关系，求解中不引入随机误差，减少了定位解算中的误差源，提高了椭圆定位的整体解算精度。

图5呈现了空间目标圆向像平面的投影示意图。在空间中尺寸相等的两个目标圆能够投影到像平面中形成同一椭圆图像，因此利用图像椭圆方程和目标罐罐口尺寸求解罐口位姿可得到两组解。下面结合图5对S2步骤中圆口目标罐罐口与相机之间的相对位置和轴向关系的解算过程进行介绍。

首先，圆口目标罐罐口圆在图像中成像满足透视投影原理，设u_j为图像中椭圆弧线上的点，则满足以下公式：

其中，u_j＝(u_j,v_j,1)^T表示第j个观测点投影到相机图像平面的点坐标，Ω＝(t,θ)表示相机坐标系相对于世界坐标系的平移和旋转， 分别表示第j个月面观测点在相机坐标系和世界坐标系中的齐次坐标，表示从月面观测点到图像的透视投影变换，表示与投影坐标相关的畸变参数矩阵，定义如下：

其中f_i ^u和f_i ^v表示以像素为单位的相机焦距，(u₀,v₀)为图像主点坐标，Δu_j和Δv_j表示由于径向畸变、偏心畸变和薄透镜畸变引起的投影点偏移量。

椭圆弧线点u_j在图像平面内满足椭圆方程：

其中A,B,C,D,E,F为步骤S1中求得的椭圆描述参数，转换成矩阵的形式：

将公式(2-1)带入公式(2-3)可得过相机光心和空间圆形成的椭圆锥的曲面方程：

令则椭圆锥曲面方程可写作：

在相机坐标系下，圆锥的表达式比较复杂，切割平面不变描述和计算。因此先将相机坐标系空间转换到标准空间计算，计算结果再转换到相机坐标系中。旋转过程固定椭圆锥原点仍在摄像机光心处。由Q为对称矩阵可知，存在正交矩阵P将Q对角化得：

P^TQP＝diag(λ₁,λ₂,λ₃)，

其中λ₁,λ₂,λ₃为Q的特征值，则可得在新的坐标空间中的点从而得到旋转轴为Z^cs轴的标准椭圆锥曲面方程：

λ₁X^cs+λ₂Y^cs+λ₃Z^cs＝0. (2-6)

由公式(2-6)可知，三个系数λ₁,λ₂,λ₃必然有两个同号，且与另一个异 号。由于λ₁,λ₂,λ₃和特征向量矩阵P的正负取值的问题，使得λ₁,λ₂,λ₃有多组解。设Q的特征值为κ₁,κ₂,κ₃，与只对应的特征向量为v₁,v₂,v₃。设κ₁,κ₂同号且|κ₁|＞|κ₂|，则λ₁＝κ₁，λ₂＝κ₂，λ₃＝κ₃。令P＝[p₁,p₂,p₃]，若则p₁＝v₁，否则p₁＝-v₁。p₂＝v₂，p₂＝v₂×v₃。

基于标准空间中的椭圆锥曲面方程，可求得圆心位置和圆所在平面的法向量为：

将公式(2-7)和公式(2-8)的结果转换到摄像机坐标系得：

即为目标罐罐口与相机之间的相对位置和轴向关系。令分别表示末端执行器坐标系到相机坐标系的齐次变换矩阵和旋转矩阵，将上述相对位置和轴向关系变换到机械臂末端执行器坐标系得到目标罐罐口与末端执行器之间的两组相对位置和轴向关系:

可选地，作为本发明另一个实施例，S3，根据所述相对位置和轴向关系以及机械臂构形约束，确定执行器的精确位姿和机械臂各关节的角度包括：

根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且非第一关节轴相互平行的机械臂构形约束，确定 机械臂各关节的关节角度和执行器位姿，得到两组执行器位姿及其对应的关节角度；其中，所述末轴垂面为与末端关节轴垂直的执行器所在的平面；

将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量，舍弃两组执行器位姿及其对应关节角度中偏差量大的一组，将偏差量小的一组执行器位姿作为执行器当前位姿，相应地获得当前机械臂各关节的关节角度；将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。

该实施例中，通过引入机械臂构形约束，能够利用椭圆定位得到的轴向信息解算机械臂的各关节角和执行器位姿，消除椭圆特征定位轴向时存在的姿态不确定性问题，进一步利用机械臂关节角和执行器位姿先验信息消除单一椭圆特征定位的二义性问题，从而得到唯一的相对位置和姿态解。

可选地，作为本发明另一个实施例，根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且非第一关节轴相互平行的机械臂构形约束，确定机械臂各关节的关节角度和执行器位姿，得到两组执行器位姿及其对应的关节角度包括：

根据机械臂非第一关节的轴线垂直于第一关节的特性，在末轴垂面上构建简化的机械臂等效模型；将所述第二轴向关系分解为末轴垂面的投影分量和法线分量，建立两组分量与关节角的变换方程，解算第一关节的关节角度和非第一关节的关节角度之和，进而计算执行器相对于机械臂基座的旋转变换矩阵，得到执行器姿态；

利用所述旋转变换矩阵、圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置，确定所述执行器相对于机械臂基座的相对位置，得到执行器位置；其中，所述执行器位姿包括执行器姿态和执行器位置；

根据中间关节的关节角度共面的构形特点，将中间关节的关节角度投影 到所述末轴垂面上，构建中间关节的关节角度与机械臂末端坐标位置的等式方程，解算中间关节的关节角度，进而求得末端关节的关节角度。其中，非第一关节为第一关节以外的其他关节。其中，中间关节为除第一关节和末端关节以外的其他关节。

该实施例中，结合机械臂的构形特点，将机械臂逆运动学求解中的各关节的关节角度进行解耦处理，将多元联合方程组求解问题化简为多个一元方程的递推求解，简化了求解过程、降低了求解难度，使得仅依靠位置信息和不完全的姿态信息(轴向信息)即能完成所有关节角的逆运动学求解。

利用机械臂构形约束和视觉测量的执行器中心坐标计算机械臂各关节角度的方法分为三步实现。以下以四自由度机械臂做如下说明：首先根据机械臂二、三、四关节轴线垂直于第一关节的特性，在与第四关节轴垂直的执行器所在平面(末轴垂面)上构建简化的机械臂等效模型，将第二轴向关系分解为该平面的投影分量和法线分量，建立两组分量与关节角的变换方程，解算第一关节的关节角度和二、三、四关节的关节角度之和，进而计算机械臂末端执行器相对于机械臂基座的旋转变换矩阵，即机械臂末端执行器姿态；再利用该旋转变换矩阵和圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置，确定执行器相对于机械臂基座的位置，即执行器位置；最后根据第二、三关节的关节角度共面的构形特点，将第二、三关节的关节角度投影到与第四关节轴垂直的执行器所在平面(末轴垂面)上，构建第二、三关节的关节角度与机械臂末端坐标位置的等式方程，解算第二关节和第三关节的关节角度，进而求得第四关节的关节角度。具体介绍如下。

结合图6、图7介绍机械臂的构形及其简化的等效构形如下。如图6所示，机械臂第二、第三、第四关节轴相互平行且与第一关节轴垂直，从而可以将图6中的原始的机械臂构形等效为图7中的简化模型。设第一、二、三、四关节的关节角分别为θ₁,θ₂,θ₃,θ₄，关节中心之间在沿关节轴向方向偏移分别 为d₁,d₂,d₃,d₄，在垂直于关节轴向方向的偏移分别为a₁,a₂,a₃,a₄。其中d₁,d₂,d₃,d₄、a₁,a₂,a₃,a₄均为已知量，a₁＝a₄＝0，θ₁,θ₂,θ₃,θ₄为运动可变量。由于机械臂为细长金属结构，其第一、第二、第三关节在运动过程中负荷较重，可能存在间隙使得θ₁,θ₂,θ₃,θ₄与实际控制量产生偏差；同时一、二关节和二、三关节之间的臂杆为细长臂杆，可能发生微小形变，使得a₂,a₃与实测值存在微小偏差。在图7的简化等效模型中，第一、四关节与原始机械臂构形相同，第二、三关节与连杆简化成为两根连杆，连杆1为可伸缩连杆，长度可变，连杆2为定长连杆，长度为d₂+d₃。该简化更加有利于后续对机械臂各关节角的解耦求解。

下面结合图8至图10，介绍根据视觉测量的执行器中心坐标求解准确的θ₁,θ₂,θ₃,θ₄，校正机械臂运动偏差的方法如下。在机械臂基座、四个关节中心、相机光心和圆口目标罐罐口中心建立坐标系如图8所示。第一关节中心坐标系由基座坐标系沿z₀轴平移距离d₁得到，随着第一关节的运动x₁轴相对于x₀轴发生旋转。第二、三、四关节中心坐标系的z轴相互平行且始终与z₀(z₁)垂直，随着各个关节的运动第二、三、四关节中心坐标系的x轴之间夹角发生变化。设机械臂末端执行器坐标系与第四关节中心坐标系重合。定义和分别为机械臂基座坐标系到第i关节中心坐标系、第i关节中心坐标系到第j关节中心坐标系、机械臂基座坐标系到圆口目标罐坐标系、机械臂基座坐标系到末端执行器坐标系和末端执行器坐标系到相机坐标系的齐次变换矩阵， 分别为对应五类坐标系变换的旋转矩阵， 为对应五类坐标系变换的平移向量。上述变量中，和均通过预先标定的方式确定为已知量，而和可通过机械臂的构形状态近似计算得到，然而由于存在机械臂关节末端位姿误差和臂杆柔性误差，使得求得的和存在一定误差，需要根据测量结果校正计算得到。下面介绍求 取和的过程。

沿圆口目标罐坐标系z轴的单位向量在机械臂基座坐标系中表达为

根据步骤2中的测量相机坐标系中的计算结果【公式(2-10)和公式(2-11)】，将其变换到机械臂末端执行器坐标系得:

由式(3-1)-(3-3)可知：

图8给出了机械臂基座及各个关节的坐标系定义，根据机械臂的构形约束及坐标系定义可知，其中θ_s＝θ₂+θ₃+θ₄。R_z(θ₁)为单位正交矩阵，则可做如下变换：

对上述矩阵方程展开得：

上述方程中的后两个等式通过三角函数关系可变换得到关于sinθ₁和sinθ_s的一元二次方程，根据一元二次方程求根公式分别求得θ₁和θ_s，各有2个解。再根据第一个等式筛选得到唯一的θ₁和θ_s，从而求得旋转变换矩阵根据公式(3-5)即可求得

下面介绍根据结合机械臂构形求解θ₂,θ₃的方法。将变换到第一关节中心坐标系得：

将已知量带入公式(3-7)并进行展开运算得：

将公式(3-8)中的前两个等式联立构建关于θ₂和θ₂+θ₃为自变量的方程组，消去θ₂+θ₃可得到关于θ₂的一元二次方程，可得到2组解，比对2组解与已知的机械臂构形状态之间的相似关系，即可求得唯一的θ₂，进而求得θ₃，利用θ₄＝θ_s-θ₂-θ₃得到θ₄。从而完成了对θ₁,θ₂,θ₃,θ₄的求解。

由于在公式(2-9)和(2-10)中，目标罐罐口中心坐标和法线为两组解，因此求得的θ₁,θ₂,θ₃,θ₄也有两组解。将这两组解分别与机械臂目标状态进行比较计算偏差，选取偏差量较小的一组解作为求得的精确位姿。根据机械臂状态与精确位姿偏差量，向减小偏差的方向调整机械臂运动。其中，机械臂目标状态是指机械臂末端执行器抵达目标位姿时对应的机械臂各关节的关节角度，可以根据机械臂末端执行器的位姿利用逆运动学求解得到。

可选地，作为本发明另一个实施例，通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像时，确定所述视觉相机的光轴方向与圆口目标罐罐口法线方向的夹角，使所述夹角与执行器的姿态偏差量的差值大于预设值；其中所述执行器的位姿偏差量包括位置偏差量和姿态偏差量。

该实施例中，通过固定在末端执行器末端的手眼相机拍摄圆口目标罐图像时，要求手眼相机的光轴方向与目标罐口法线方向夹角远大于执行器的姿态偏差量，如可大于执行器姿态偏差量的5倍。该要求能够保证S3步骤中根据末端执行器位姿偏差量大小，舍弃偏差大的一组，只保留唯一解的正确性。通过这一设定能够确保执行器姿态误差量能够被椭圆特征准确反映，不发生因光轴与目标罐罐口法线夹角太小导致执行器姿态误差量难以分辨的 情况，确保了椭圆特征定位的唯一性和可靠性。

需要说明的是，上述实施例中的机械臂可以为如图1所示的机械臂，该机械臂可以为四自由度机械臂，其四个关节皆为旋转关节，第二、三、四关节平行，与水平多关节机械臂SCARA的三个旋转关节构造相同，并与第一轴垂直。一、二关节和三、四关节组合紧凑，关节轴距离极小，二、三关节和三、四关节之间通过长臂连杆连接。末端执行器安装固定于四自由度机械臂的第四关节处，安装固定方向与第四关节垂直，能够绕第四关节转动；视觉相机可以为手眼相机，安装于执行器上，轴线与执行器轴线平行；圆口目标罐安装于机械臂可达的范围内，固定安装，罐口裸露向上，能够被机械臂末端的视觉相机观测，罐口尺寸已知。

上文结合图1至图10，详细描述了根据本发明实施例提供基于椭圆特征的机械臂末端位姿误差校正方法。下面结合图11，详细描述本发明实施例提供基于椭圆特征的机械臂末端位姿误差校正系统。

图11给出了本发明实施例提供的一种基于椭圆特征的机械臂末端位姿误差校正系统的示意性结构框图。如图11所示，该系统包括图像获取模块、处理模块和控制模块。

图像获取模块通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像；处理模块从所述圆口目标罐的图像中提取目标椭圆；根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系；根据所述相对位置和轴向关系以及机械臂构形约束，确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量；控制模块根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂，直至执行器的位姿偏差量小于预设值。

该实施例中，利用机械臂末端的视觉相机获取目标罐的图像，从图像中获取单个目标椭圆，根据目标椭圆的椭圆特征确定机械臂末端执行器与 目标罐的相对位置和轴向关系，进而确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量，根据位姿偏差量设计机械臂操作调整策略。与现有的视觉引导的误差校正方法相比，该方法能够利用机械臂末端视觉相机获取图像中的单个椭圆特征和机械臂构形先验信息计算目标罐的位置和姿态，不仅考虑机械臂末端位姿偏差，还考虑机械臂各关节的偏差量，解决了单个椭圆特征定位存在二义性且无法确定轴向旋转姿态的困难，可为机械臂抓取样品并放入目标罐中提供精确引导，校正因局部柔性或关节间隙引起的控制误差，使遥操作人员准确掌握机械臂末端执行器与目标罐的相对状态和机械臂各关节的当前状态，进而有效控制机械臂的精准操作，提高操作精度。

可选地，作为本发明另外一个实施例，所述处理模块具体用于：提取所述图像中的轮廓，通过弧段筛选算法从所述轮廓中剔除无效弧段，选择有效弧段，利用最小二乘算法对每个有效弧段进行椭圆拟合，合并交叠椭圆，剔除无效椭圆，获取目标椭圆。

可选地，作为本发明另一个实施例，所述处理模块具体用于：获取所述目标椭圆的椭圆方程描述参数，利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程；利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面，根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系；根据手眼标定关系将所述第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。

可选地，作为本发明另一个实施例，所述处理模块具体用于：根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且非第一关节轴相互平行的机械臂构形约束，确定机械臂各关节的关节角度和执行器位姿，得到两组执行器位姿及其对应的关节角度；其中， 所述末轴垂面为与末端关节轴垂直的执行器所在的平面；将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量，舍弃两组执行器位姿及其对应关节角度中偏差量大的一组，将偏差量小的一组执行器位姿作为执行器当前位姿，相应地获得当前机械臂各关节的关节角度；将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。

可选地，作为本发明另一个实施例，处理模块具体用于，根据机械臂非第一关节的轴线垂直于第一关节的特性，在末轴垂面上构建简化的机械臂等效模型；将所述第二轴向关系分解为末轴垂面的投影分量和法线分量，建立两组分量与关节角的变换方程，解算第一关节的关节角度和非第一关节的关节角度之和，进而计算执行器相对于机械臂基座的旋转变换矩阵，得到执行器姿态；利用所述旋转变换矩阵、圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置，确定所述执行器相对于机械臂基座的相对位置，得到执行器位置；其中，所述执行器位姿包括执行器姿态和执行器位置；根据中间关节的关节角度共面的构形特点，将中间关节的关节角度投影到所述末轴垂面上，构建中间关节的关节角度与机械臂末端坐标位置的等式方程，解算中间关节的关节角度，进而求得末端关节的关节角度。

可选地，作为本发明另一个实施例，图像获取模块通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像时，确定所述视觉相机的光轴方向与圆口目标罐罐口法线方向的夹角，使所述夹角与执行器的姿态偏差量的差值大于预设值；其中所述执行器的位姿偏差量包括位置偏差量和姿态偏差量。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。

作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本发明实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于椭圆特征的机械臂末端位姿误差校正方法，其特征在于，包括：

通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像，从所述圆口目标罐的图像中提取目标椭圆；

根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系；

根据所述相对位置和轴向关系以及机械臂构形约束，确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量；

根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂，直至执行器的位姿偏差量小于预设值。

2.根据权利要求1所述的方法，其特征在于，所述从所述圆口目标罐的图像中提取目标椭圆包括：提取所述图像中的轮廓，通过弧段筛选算法从所述轮廓中剔除无效弧段，选择有效弧段，利用最小二乘算法对每个有效弧段进行椭圆拟合，合并交叠椭圆，剔除无效椭圆，获取目标椭圆。

3.根据权利要求1所述的方法，其特征在于，所述根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系包括：

获取所述目标椭圆的椭圆方程描述参数，利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程；

利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面，根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系；

根据手眼标定关系将所述两组第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。

4.根据权利要求3所述的方法，其特征在于，所述根据所述相对位置和轴向关系以及机械臂构形约束，确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量包括：

根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且非第一关节轴相互平行的机械臂构形约束，确定机械臂各关节的关节角度和执行器位姿，得到两组执行器位姿及其对应的关节角度；其中，所述末轴垂面为与末端关节轴垂直的执行器所在的平面；

将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量，舍弃两组执行器位姿及其对应关节角度中偏差量大的一组，将偏差量小的一组执行器位姿作为执行器当前位姿，相应地获得当前机械臂各关节的关节角度；将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。

5.根据权利要求4所述的方法，其特征在于，所述根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且非第一关节轴相互平行的机械臂构形约束，确定机械臂各关节的关节角度和执行器位姿，得到两组执行器位姿及其对应的关节角度包括：

根据机械臂非第一关节的轴线垂直于第一关节的特性，在末轴垂面上构建简化的机械臂等效模型；将所述第二轴向关系分解为末轴垂面的投影分量和法线分量，建立两组分量与关节角的变换方程，解算第一关节的关节角度和非第一关节的关节角度之和，进而计算执行器相对于机械臂基座的旋转变换矩阵，得到执行器姿态；

利用所述旋转变换矩阵、圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置，确定所述执行器相对于机械臂基座的相对位置，得到执行器位置；其中，所述执行器位姿包括执行器姿态和执行器位置；

根据中间关节的关节角度共面的构形特点，将中间关节的关节角度投影到所述末轴垂面上，构建中间关节的关节角度与机械臂末端坐标位置的等式方程，解算中间关节的关节角度，进而求得末端关节的关节角度。

6.根据权利要求1至5任一项所述的方法，其特征在于，所述通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像时，确定所述视觉相机的光轴方向与圆口目标罐罐口法线方向的夹角，使所述夹角与执行器的姿态偏差量的差值大于预设值；其中所述执行器的位姿偏差量包括位置偏差量和姿态偏差量。

7.一种基于椭圆特征的机械臂末端位姿误差校正系统，其特征在于，包括：

图像获取模块，用于通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像；

处理模块，用于从所述圆口目标罐的图像中提取目标椭圆；

用于根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系；

用于根据所述相对位置和轴向关系以及机械臂构形约束，确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量；

控制模块，用于根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂，直至执行器的位姿偏差量小于预设值。

8.根据权利要求7所述的系统，其特征在于，所述处理模块具体用于：提取所述图像中的轮廓，通过弧段筛选算法从所述轮廓中剔除无效弧段，选择有效弧段，利用最小二乘算法对每个有效弧段进行椭圆拟合，合并交叠椭圆，剔除无效椭圆，获取目标椭圆。

9.根据权利要求7所述的系统，其特征在于，所述处理模块具体用于：

获取所述目标椭圆的椭圆方程描述参数，利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程；

利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面，根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系；

根据手眼标定关系将所述第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。

10.根据权利要求9所述的系统，其特征在于，所述处理模块具体用于：

根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且非第一关节轴相互平行的机械臂构形约束，确定机械臂各关节的关节角度和执行器位姿，得到两组执行器位姿及其对应的关节角度；其中，所述末轴垂面为与末端关节轴垂直的执行器所在的平面；

将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量，舍弃两组执行器位姿及其对应关节角度中偏差量大的一组，将偏差量小的一组执行器位姿作为执行器当前位姿，相应地获得当前机械臂各关节的关节角度；将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。