[發明專利]一種空間角度姿態計算方法

有效

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

1.一種空間角度姿態計算方法，應用于機械旋轉裝置中，所述機械旋轉裝置包括依次轉動連接的根元件、腕元件、樞元件和梢元件，根元件和腕元件之間的運動副為橈關節，腕元件和樞元件之間的運動副為腕關節，樞元件和梢元件之間的運動副為掌關節，梢元件在樞元件內繞梢軸旋轉，其特征在于，梢元件上安裝有慣性測量元件；

所述空間角度姿態計算方法包括：

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

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

利用慣性測量元件各軸在系統坐標系中的初始方向矢量和在導航坐標系中的初始方向矢量，計算從導航坐標系到系統坐標系的轉換矩陣；

系統工作時，利用慣性測量元件輸出的實時歐拉角，計算慣性測量元件各軸在導航坐標系內的實時方向矢量；

利用從導航坐標系到系統坐標系的轉換矩陣，計算慣性測量元件各軸在系統坐標系中的實時方向矢量；

獲取根元件各軸在系統坐標系內的實時方向矢量；

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

2.根據權利要求1所述的空間角度姿態計算方法，其特征在于，計算慣性測量元件各軸在系統坐標系中的實時方向矢量的步驟具體包括：

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

利用慣性測量元件各軸在導航坐標系中的初始方向矢量和慣性測量元件各軸在系統坐標系中的初始方向矢量計算從導航坐標系到系統坐標系的轉換矩陣；

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

利用慣性測量元件各軸的方向矢量和實時計算得到的從梢坐標系到導航坐標系的轉換矩陣計算慣性測量元件各軸在導航坐標系中的實時方向矢量；

利用從導航坐標系到系統坐標系的轉換矩陣計算慣性測量元件各軸在系統坐標系中的實時方向矢量。

3.根據權利要求2所述的空間角度姿態計算方法，其特征在于，從導航坐標系到系統坐標系的轉換矩陣Tns的計算公式如下：

Tns=[imu_ENU_10,imu_ENU_20,imu_ENU_30]^-1×[imu_S_10,imu_S_20,imu_S_30]

其中，imu_ENU_n0為慣性測量元件各軸在導航坐標系中的初始方向矢量，imu_S_n0為慣性測量元件各軸在系統坐標系中的初始方向矢量，n=1,2,3。

4.根據權利要求1所述的空間角度姿態計算方法，其特征在于，根元件的坐標系為（X,Y,Z），梢元件的坐標系為（x,y,z），運用空間矢量運算計算慣性測量元件與根元件在各自由度上的轉角的步驟中，計算的轉角包括橈關節轉角、腕關節轉角和掌關節轉角；

橈關節轉角為梢元件z軸在根元件XZ平面內的投影與根元件Z軸之間的夾角；

腕關節轉角為梢元件z軸與根元件XZ平面之間的夾角；

掌關節轉角為根元件Y軸在梢元件xy平面內的投影與梢元件x軸之間的夾角。