[發明專利]一種關節角的初始化方法、裝置及飛行器

在審

• 申請號: 202010801302.4
• 申請日: 2020-08-11
• 公開（公告）號: CN111977006A
• 公開（公告）日: 2020-11-24
• 發明（設計）人: 汪康利
• 申請（專利權）人: 深圳市道通智能航空技術有限公司
• 主分類號: B64D47/08
• 分類號: B64D47/08;B64C39/02
• 代理公司: 深圳市六加知識產權代理有限公司 44372
• 代理人: 孟麗平

1.一種關節角的初始化方法，應用于飛行器，所述飛行器設置有三軸云臺，其特征在于，所述三軸云臺包括三軸電機，分別為第一電機、第二電機以及第三電機，所述第二電機對應第二極對數，所述第三電機對應第三極對數，所述三軸云臺還包括基座端和工具端，所述工具端設置有三軸加速度計，所述方法包括：

獲取所述第一電機上電時刻對應的第一關節角以及所述第二電機上電時刻對應的第二電氣角度和第三電機上電時刻對應的第三電氣角度；

根據所述第二電機的第二電氣角度、第二極對數以及所述第三電機的第三電氣角度、第三極對數，遍歷所述第二電機和第三電機的第二關節角和第三關節角，確定所述第一電機、第二電機以及第三電機的關節角組合；

確定所述關節角組合對應的姿態四元數以及所述基座端的姿態四元數，根據所述基座端的姿態四元數以及關節角組合的姿態四元數，確定所述工具端的姿態四元數；

根據所述工具端的姿態四元數，結合所述工具端的三軸加速度計的讀數，確定所述三軸電機的初始化關節角組合。

2.根據權利要求1所述的方法，其特征在于，所述基座端設置有三軸加速度計，所述確定所述基座端的姿態四元數，包括：

獲取所述基座端的三軸加速度計的讀數，固定所述基座端的偏航角，根據所述基座端的三軸加速度計的讀數，計算所述基座端的俯仰角和橫滾角；

根據所述基座端的偏航角、俯仰角和橫滾角，確定所述基座端的姿態四元數。

3.根據權利要求1所述的方法，其特征在于，所述確定所述第一電機、第二電機以及第三電機的關節角組合之后，所述方法還包括：對所述第一電機、第二電機以及第三電機的關節角組合進行篩選，確定篩選后的關節角組合，所述確定所述關節角組合對應的姿態四元數，包括：

確定每一篩選后的關節角組合對應的姿態四元數。

4.根據權利要求1所述的方法，其特征在于，所述獲取上電時刻所述第一電機對應的第一關節角，包括：

通過自檢碰限位的方式，確定所述第一電機對應的第一關節角。

5.根據權利要求1所述的方法，其特征在于，所述方法還包括：

計算所述基座端的加速度計模值和工具端的加速度計模值；

根據所述基座端的加速度計模值和工具端的加速度計模值，確定所述云臺是否處于靜止狀態。

6.根據權利要求1所述的方法，其特征在于，所述根據所述基座端的姿態四元數以及關節角組合的姿態四元數，確定所述工具端的姿態四元數，包括：

對所述基座端的姿態四元數和所述關節角組合的姿態四元數進行四元數乘法，確定所述工具端的姿態四元數。

7.根據權利要求1所述的方法，其特征在于，所述根據所述工具端的姿態四元數，結合所述工具端的三軸加速度計的讀數，確定所述三軸電機的初始化關節角組合，包括：

根據所述工具端的姿態四元數，確定旋轉矩陣；

根據所述工具端的三軸加速度計的讀數，生成重力向量；

根據所述旋轉矩陣和所述重力向量，確定所述三軸電機的初始化關節角組合。

8.根據權利要求7所述的方法，其特征在于，所述根據所述旋轉矩陣和所述重力向量，確定所述三軸電機的初始化關節角組合，包括：

獲取所述旋轉矩陣的列向量；

對所述旋轉矩陣的列向量和所述重力向量進行點乘運算，生成多個點乘結果；

將點乘結果的最大值對應的第一關節角、第二關節角以及第三關節角作為所述三軸電機的初始化關節角組合。