[發(fā)明專利]一種空間角度姿態(tài)計算方法

有效

• 申請?zhí)枺?/td>: 202011175931.7
• 申請日: 2020-10-29
• 公開（公告）號: CN111998822B
• 公開（公告）日: 2021-01-15
• 發(fā)明（設(shè)計）人: 辜紀文;林嘉;辜正濤
• 申請（專利權(quán)）人: 江西明天高科技股份有限公司;南昌嘉研科技有限公司
• 主分類號: G01C1/00
• 分類號: G01C1/00;G01C21/20
• 代理公司: 北京清亦華知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11201
• 代理人: 彭琰

本發(fā)明提供了一種空間角度姿態(tài)計算方法，包括：獲取IMU各軸在梢坐標系中的初始方向矢量和初始歐拉角；計算IMU各軸在導(dǎo)航坐標系中的初始方向矢量；利用IMU各軸在系統(tǒng)坐標系和導(dǎo)航坐標系中的初始方向矢量，計算從導(dǎo)航坐標系到系統(tǒng)坐標系的轉(zhuǎn)換矩陣；系統(tǒng)工作時，利用IMU輸出的實時歐拉角，計算IMU各軸在導(dǎo)航坐標系內(nèi)的實時方向矢量；利用轉(zhuǎn)換矩陣，計算IMU各軸在系統(tǒng)坐標系中的實時方向矢量；獲取根元件各軸在系統(tǒng)坐標系內(nèi)的實時方向矢量；運用空間矢量運算計算慣性測量元件與根元件在各自由度上的轉(zhuǎn)角。本發(fā)明能夠解決現(xiàn)有技術(shù)只能測量一個維度上的角度值，不適用于多旋轉(zhuǎn)自由度零件的問題。

技術(shù)領(lǐng)域

本發(fā)明涉及數(shù)據(jù)處理技術(shù)領(lǐng)域，特別是涉及一種空間角度姿態(tài)計算方法。

背景技術(shù)

空間角度姿態(tài)計算是機械系統(tǒng)中的常見需求，現(xiàn)有技術(shù)中，有多種不同類型的角度測量傳感器，如電位型、霍爾型、柵格型等，來實現(xiàn)空間角度姿態(tài)的測量和計算，這些角度傳感器各自的工作原理不盡相同，但利用這些角度傳感器進行測量時，都只能測量一個維度上的角度值。

對于多旋轉(zhuǎn)自由度零件，例如，根、轉(zhuǎn)、樞、梢四個零件通過三個旋轉(zhuǎn)自由度（d1、d2、d3）相互連接，形成如圖1所示的類似人手執(zhí)筆的結(jié)構(gòu)，在實際應(yīng)用中，需要測量d1、d2、d3的旋轉(zhuǎn)角度，使用傳統(tǒng)的角度測量傳感器將無法實現(xiàn)d1、d2、d3旋轉(zhuǎn)角度的測量，此外，角度傳感器在工作中必須以一側(cè)零件為固定參考，測量另一側(cè)零件的相對轉(zhuǎn)動，而無法感知其參考點自身的姿態(tài)變化，根零件自身的位置、朝向等的變化也需要進行測量，這些因素都限制了傳統(tǒng)角度傳感器的使用。

發(fā)明內(nèi)容

本發(fā)明的目的在于提供一種空間角度姿態(tài)計算方法，以解決現(xiàn)有技術(shù)只能測量一個維度上的角度值，不適用于多旋轉(zhuǎn)自由度零件的問題。

本發(fā)明提供一種空間角度姿態(tài)計算方法，應(yīng)用于機械旋轉(zhuǎn)裝置中，所述機械旋轉(zhuǎn)裝置包括依次轉(zhuǎn)動連接的根元件、腕元件、樞元件和梢元件，根元件和腕元件之間的運動副為橈關(guān)節(jié)，腕元件和樞元件之間的運動副為腕關(guān)節(jié)，樞元件和梢元件之間的運動副為掌關(guān)節(jié)，梢元件在樞元件內(nèi)繞梢軸旋轉(zhuǎn)，梢元件上安裝有慣性測量元件；

所述空間角度姿態(tài)計算方法包括：

在初始狀態(tài)下對系統(tǒng)進行標定，獲取初始狀態(tài)下慣性測量元件各軸在梢坐標系中的初始方向矢量，并讀取此時慣性測量元件輸出的初始歐拉角；

通過慣性測量元件各軸在梢坐標系中的初始方向矢量和初始歐拉角，計算慣性測量元件各軸在導(dǎo)航坐標系中的初始方向矢量；

利用慣性測量元件各軸在系統(tǒng)坐標系中的初始方向矢量和在導(dǎo)航坐標系中的初始方向矢量，計算從導(dǎo)航坐標系到系統(tǒng)坐標系的轉(zhuǎn)換矩陣；

系統(tǒng)工作時，利用慣性測量元件輸出的實時歐拉角，計算慣性測量元件各軸在導(dǎo)航坐標系內(nèi)的實時方向矢量；

利用從導(dǎo)航坐標系到系統(tǒng)坐標系的轉(zhuǎn)換矩陣，計算慣性測量元件各軸在系統(tǒng)坐標系中的實時方向矢量；

獲取根元件各軸在系統(tǒng)坐標系內(nèi)的實時方向矢量；

運用空間矢量運算計算慣性測量元件與根元件在各自由度上的轉(zhuǎn)角。

進一步地，計算慣性測量元件各軸在系統(tǒng)坐標系中的實時方向矢量的步驟具體包括：

根據(jù)初始歐拉角和慣性測量元件各軸的方向矢量，利用初始的從梢坐標系到導(dǎo)航坐標系的轉(zhuǎn)換矩陣計算慣性測量元件各軸在導(dǎo)航坐標系中的初始方向矢量；

利用慣性測量元件各軸在導(dǎo)航坐標系中的初始方向矢量和慣性測量元件各軸在系統(tǒng)坐標系中的初始方向矢量計算從導(dǎo)航坐標系到系統(tǒng)坐標系的轉(zhuǎn)換矩陣；

當慣性測量元件輸出實時歐拉角時，實時計算從梢坐標系到導(dǎo)航坐標系的轉(zhuǎn)換矩陣；