[發明專利]一種基于四元數EKF和未標定手眼系統的物體位姿估算方法

在審

• 申請號: 202110085490.X
• 申請日: 2021-01-13
• 公開（公告）號: CN114764830A
• 公開（公告）日: 2022-07-19
• 發明（設計）人: 黃正軍;周梅芳;方韶華
• 申請（專利權）人: 金華職業技術學院
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/77
• 代理公司: 暫無信息
• 代理人: 暫無信息

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在相機坐標系中的坐標。