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

权利要求书

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

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

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

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

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

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

2.根据权利要求1所述的一种基于四元数EKF和未标定手眼系统的物体位姿估算方法，其特征在于，所述步骤S1的具体过程包括：

定义手眼视觉伺服系统状态向量为：

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

X_k＝A_kX_k-1+w_k-1 (2)

式(2)中：A为系统矩阵，w_k-1为零均值系统高斯白噪声，其协方差阵为Q_k-1。

公式(1)中用四元数来表示抓取目标在相机坐标系中的姿态角，对四元数微分求积分即可得到更新的四元数。对四元数微分方程展开可得：

其中：

假设采样周期内所有角速度不变，对公式(3)求积分可得：

解上式得出：

设：

将公式(6)、(7)代入公式(2)得系统状态向量X的离散时间模型：

上式中Q_tran为4x4四元数更新矩阵，T为采样时间。系统矩阵A如(9)所示。

式(9)中Q_w为表示四元数变化的4x3矩阵，其表达式如下：

上述矩阵每列的计算方法如式(11)所示：

其中：

。

3.根据权利要求1所述的一种基于四元数EKF和未标定手眼系统的物体位姿估算方法，其特征在于，所述步骤S2的具体过程如下：

取抓取目标上特征点在成像平面的像素坐标作为观测向量，建立扩展卡尔曼滤波器的观测方程为：

S_k＝G(X_k)+v_k (15)

其中：S_k为抓取目标特征点在成像平面的像素坐标向量，为保证位姿估算的鲁棒性，采用4个特征点来估算抓取目标的位姿。G(X_k)为描述相机成像模型的函数，对于普通针孔相机其成像平面的像素坐标计算如下：

式(16)、(17)中u、v为特征点在成像平面的像素坐标，X、Y、Z为特征点在相机坐标系中的坐标，f为相机焦距，dsw、dsh为成像平面横向与纵向单位像素长度，u₀、v₀为成像平面中心坐标。

结合式(16)、(17)观测方程表达式如下：

由于式(18)的非线性性，不能直接进行卡尔曼滤波处理，因此需对式(18)求取雅可比矩阵进行线性化处理，得系统量测矩阵为：

式(19)中：(i＝1～4)为特征点i在相机坐标系中的坐标。