[发明专利]一种基于四元数EKF和未标定手眼系统的物体位姿估算方法

说明书

本发明提出了一种基于四元数EKF和未标定手眼系统的物体位姿估算方法。以四元数描述物体姿态角，建立四元数EKF状态方程和观测方程，结合EKF算法估算物体位姿，再将求得的四元数转换成欧拉角，最后输出准确物体位姿。本发明的方法有效克服了欧拉角描述物体姿态存在奇异值且计算量大的缺点，相较于传统EKF具有更快的运算速度和更高的估算精度，可以保证物体位姿估算的精确性和实时性。

技术领域

本发明涉及机器人视觉伺服技术领域，具体涉及一种基于四元数EKF和未标定手眼系统的物体位姿估算方法。

背景技术

随着人工智能及机器人技术的迅猛发展，机器人执行任务的复杂性和多样性不断提高，具备感知能力、能与外界互动的智能机器人被公认为今后机器人发展的方向，实现自主定位和导航是智能机器人的基本要求和核心技术。在目前比较成熟的定位和导航技术中，视觉伺服技术因其信息量大、适用范围广、定位或跟踪精度高等特点而得到广泛应用。考虑到技术及成本等多方面因素，无标定单目视觉在市场上受到普遍关注，但单目视觉无法直接得到目标的三维信息，必须采用合理的算法来进行位姿估算。

为了实现三维结构目标物体的快速识别和位姿估算，进而提升智能机器人的自动化和智能化水平，一些机器人视觉伺服解决方案被相继提出。例如，授权公告号为CN104596502B的中国发明专利就公开了一种基于CAD模型与单目视觉的物体位姿测量方法，该方法通过模板拍摄摄像机运动假设和迭代计算得到假设运动的模板拍摄摄像机与固定摄像机之间的运动关系，得到假设运动的模板拍摄摄像机和固定摄像机组成的双目系统外参数后，对目标进行三维重建，得到目标的三维点云数据，并与包含物体三维结构信息CAD模型进行配准，得到当前世界坐标系下目标物体位姿与CAD模型的对应关系，准确计算出目标物体的位姿。此方法虽能准确估算物体位姿，但前期需建立物体CAD模型，并对目标物体进行三维重建，算法复杂计算量大，不利于位姿估算的实时性。

EKF利用泰勒级数展开的方法将非线性系统线性化，进而采用卡尔曼滤波的基本算法进行估算状态更新，是较为优化的位姿估算方法。EKF估算物体位姿时，通常用欧拉角来描述目标姿态，欧拉角法虽描述简单，无冗余参数，但存在奇异值，且计算量大，不利于位姿估算的精确性与实时性。

发明内容

针对上述问题，本发明的目的是提供一种基于四元数EKF和未标定手眼系统的物体位姿估算方法，不仅可以避免欧拉角的奇异问题，而且计算量小，运算速度快，有利于位姿估算的实时性。

为实现上述目的，本发明提供的技术方案如下：

一种基于四元数EKF和未标定手眼系统的物体位姿估算方法，其特征在于，包括如下步骤：

步骤S1：建立四元数EKF状态方程；

步骤S2：建立四元数EKF观测方程；

步骤S3：采用四元数EKF算法估算物体位姿；

步骤S4：四元数欧拉角换算；

步骤S5：输出物体位姿信息。

更进一步的，所述步骤S1的具体过程包括：

在手眼系统中，相机相对于机械手有固定的位置关系并随着机械手做空间运动，手眼系统的动力学模型描述抓取目标相对于手眼相机的相对运动，定义系统状态向量为：

式(1)中q₀～q₃为描述抓取目标相对于相机坐标系旋转的四元数，w_x、w_y、w_z为目标旋转角速度在相机坐标系中的分量，抓取目标的平移运动由其余6个系统状态表示。假设相机运动缓慢并且在每一个采样周期内速度不变，则抓取目标位姿的一阶导可设为常数，二阶导则作为系统噪声包含在系统动力学模型中。卡尔曼滤波的系统状态方程如下：