[發明專利]一種集成陀螺儀的機械手末端空間姿態感知球及方法

有效

• 申請號: 202010675235.6
• 申請日: 2020-07-14
• 公開（公告）號: CN111923040B
• 公開（公告）日: 2022-01-11
• 發明（設計）人: 馬小龍;孟海良;韋威;鮑官軍
• 申請（專利權）人: 浙江工業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 杭州杭誠專利事務所有限公司 33109
• 代理人: 尉偉敏

1.一種集成陀螺儀的機械手末端空間姿態感知球，其特征在于，包括外殼，所述外殼內部設有數據采集處理結構，所述數據采集處理結構包括依次相連的電子陀螺儀(2)、數據處理CPU(3)和數據發送藍牙(7)，還包括電池(4)，所述電池(4)經過電壓轉換芯片(6)與數據處理CPU(3)相連，所述電池(4)處于殼體內部幾何中心位置；

一種集成陀螺儀的機械手末端空間姿態感知球的工作方法，包括以下步驟：

①采集當前的6個位姿量；

②計算加速度模長并對角加速度歸一化處理；

③計算估計方向的重力；

④計算傳感器測到的方向與參考方向的誤差，并對誤差值進行積分增益Ki倍，傳感器測到的方向與參考方向的誤差

對誤差值進行積分增益Ki倍：

增益后的值＝增益前的值+(e_x e_y e_z)·Ki

式中a_x,a_y,a_z分別表示加速度計測到的沿x軸,y軸,z軸的加速度分量，為從地理坐標系到物體坐標系下的轉換矩陣，v_x,v_y, v _z分別表示在物體坐標系下的方向余弦值；

⑤計算4個估計方向數值經過時間間隔T后的值并進行歸一化處理，4個估計方向數值經過時間間隔T后的值：

對4個估計方向進行歸一化處理得到：

式中(q₀ q₁ q₂ q₃)^T為從歐拉角轉換到四元數的值；GYR_X,GYR_Y,GYR_Z分別為陀螺儀繞x軸,y軸,z軸的角速度；norm是四元數的模長，為下一步的歸一化處理計算的模長值；

⑥計算得到感知球在地面坐標系下的三個坐標角度，

繞Z軸轉動的角度：

繞Y軸轉動的角度：

繞X軸轉動的角度：

atan為三角函數的反正切函數，asin為三角函數的反正弦函數。

2.根據權利要求1所述的一種集成陀螺儀的機械手末端空間姿態感知球，其特征在于，所述殼體包括上半球外殼(1)、下半球外殼(8)和球內部型腔(5)，所述上半球外殼(1)和下半球外殼(8)通過卡扣固定連接，所述上半球外殼(1)中集成有電子陀螺儀(2)和數據處理CPU(3)，所述下半球外殼(8)集成有電池(4)、電壓轉化芯片(6)和數據發送藍牙(7)。

3.根據權利要求1所述的一種集成陀螺儀的機械手末端空間姿態感知球，其特征在于，所述步驟①中6個位姿量包括：繞X軸旋轉的角加速度分量ACC_X，繞Y軸旋轉的角加速度分量ACC_Y，繞Z軸旋轉的角加速度分量ACC_Z，繞X軸旋轉的角速度分量GYR_X，繞Y軸旋轉的角速度分量GYR_Y，繞Z軸旋轉的角速度分量GYR_Z。

4.根據權利要求3所述的一種集成陀螺儀的機械手末端空間姿態感知球，其特征在于，所述步驟②計算加速度模長：

進行角加速度歸一化處理：

ACC_X＝ACC_X/norm,

ACC_Y＝ACC_Y/norm,

ACC_Z＝ACC_Z/norm。

5.根據權利要求1所述的一種集成陀螺儀的機械手末端空間姿態感知球，其特征在于，所述步驟③估計方向的重力計算：設置4個估計方向：

(q₀ q₁ q₂ q₃)^T＝(1 0 0 0)^T，

計算估計方向的重力

v_x＝2(q₁·q₃-q₀·q₂),

v_y＝2(q₀·q₁+q₃·q₂),

v_z＝q₀·q₀-q₁·q₁-q₂·q₂+q₃·q₃。