[發(fā)明專利]云臺(tái)電機(jī)角度和角速度估算方法、裝置、云臺(tái)及飛行器

在審

• 申請(qǐng)?zhí)枺?/td>: 201810502724.4
• 申請(qǐng)日: 2018-05-23
• 公開（公告）號(hào): CN108762324A
• 公開（公告）日: 2018-11-06
• 發(fā)明（設(shè)計(jì)）人: 徐運(yùn)揚(yáng)
• 申請(qǐng)（專利權(quán)）人: 深圳市道通智能航空技術(shù)有限公司
• 主分類號(hào): G05D13/62
• 分類號(hào): G05D13/62
• 代理公司: 深圳市六加知識(shí)產(chǎn)權(quán)代理有限公司 44372
• 代理人: 王廣濤

1.一種云臺(tái)電機(jī)角度和角速度估算方法，其特征在于，所述云臺(tái)用于搭載拍攝裝置，所述云臺(tái)包括基座及電機(jī)，所述拍攝裝置與所述基座通過所述電機(jī)進(jìn)行連接，所述拍攝裝置設(shè)置有第一慣性測量單元，所述基座設(shè)置有第二慣性測量單元，所述方法包括：

獲取所述第一慣性測量單元采集的第一角速度測量值，并獲取所述第二慣性測量單元采集的第二角速度測量值；

根據(jù)所述第一角速度測量值及所述第二角速度測量值，確定所述電機(jī)的角度；

根據(jù)所述電機(jī)的角度、所述第一角速度測量值及所述第二角速度測量值，確定所述電機(jī)的角速度。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述根據(jù)所述電機(jī)的角度、所述第一角速度測量值及所述第二角速度測量值，確定所述電機(jī)的角速度，包括：

根據(jù)電機(jī)的角度，確定第一旋轉(zhuǎn)變換矩陣及第二旋轉(zhuǎn)變換矩陣，所述第一旋轉(zhuǎn)變換矩陣為基座坐標(biāo)系到拍攝裝置坐標(biāo)系的旋轉(zhuǎn)矩陣，所述第二旋轉(zhuǎn)變換矩陣為基座坐標(biāo)系到電機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣；

根據(jù)所述第一旋轉(zhuǎn)變換矩陣、所述第二旋轉(zhuǎn)變換矩陣、所述第一角速度測量值及所述第二角速度測量值，計(jì)算得到所述電機(jī)的角速度。

3.根據(jù)權(quán)利要求2所述的方法，其特征在于，根據(jù)所述第一旋轉(zhuǎn)變換矩陣、所述第二旋轉(zhuǎn)變換矩陣、所述第一角速度測量值及所述第二角速度測量值計(jì)算得到所述電機(jī)的角速度的計(jì)算公式為：

其中，R_zxy(φ,θ,ψ)表示為第一旋轉(zhuǎn)變換矩陣；D表示為第二旋轉(zhuǎn)變換矩陣；D^-1表示為第二旋轉(zhuǎn)變換矩陣的逆矩陣；表示為第一角速度測量值；表示為第二角速度測量值；ω表示為所述電機(jī)的角速度。

4.根據(jù)權(quán)利要求3所述的方法，其特征在于，根據(jù)電機(jī)的角度確定第一旋轉(zhuǎn)變換矩陣的計(jì)算公式為：

其中，R_zxy(φ,θ,ψ)表示為第一旋轉(zhuǎn)變換矩陣；(φ,θ,ψ)表示為所述電機(jī)的角度，φ表示為所述電機(jī)的翻滾軸的轉(zhuǎn)動(dòng)角度，θ表示為所述電機(jī)的俯仰軸的轉(zhuǎn)動(dòng)角度，ψ表示為所述電機(jī)的偏航軸的轉(zhuǎn)動(dòng)角度。

5.根據(jù)權(quán)利要求3所述的方法，其特征在于，根據(jù)電機(jī)的角度確定第二旋轉(zhuǎn)變換矩陣的計(jì)算公式為：

其中，D表示為第二旋轉(zhuǎn)變換矩陣；(φ,θ,ψ)表示為所述電機(jī)的角度，φ表示為所述電機(jī)的翻滾軸的轉(zhuǎn)動(dòng)角度，θ表示為所述電機(jī)的俯仰軸的轉(zhuǎn)動(dòng)角度，ψ表示為所述電機(jī)的偏航軸的轉(zhuǎn)動(dòng)角度。

6.根據(jù)權(quán)利要求1-5任一項(xiàng)所述的方法，其特征在于，所述根據(jù)所述第一角速度測量值及所述第二角速度測量值，確定所述電機(jī)的角度，包括：

根據(jù)所述第一角速度測量值得到第一姿態(tài)四元數(shù)，并根據(jù)所述第二角速度測量值得到第二姿態(tài)四元數(shù)，其中，所述第一姿態(tài)四元數(shù)用于表示所述拍攝裝置相對(duì)于慣性系的姿態(tài)角，所述第二姿態(tài)四元數(shù)用于表示所述基座相對(duì)所述慣性系的姿態(tài)角；

根據(jù)所述第一姿態(tài)四元數(shù)與所述第二姿態(tài)四元數(shù)，得到第三姿態(tài)四元數(shù)，所述第三姿態(tài)四元數(shù)用于表示所述電機(jī)的旋轉(zhuǎn)姿態(tài)角；

根據(jù)所述第三姿態(tài)四元數(shù)，得到所述電機(jī)的角度。

7.根據(jù)權(quán)利要求6所述的方法，其特征在于，所述根據(jù)所述第一角速度測量值得到第一姿態(tài)四元數(shù)，并根據(jù)所述第二角速度測量值得到第二姿態(tài)四元數(shù)，包括：

以所述第一角速度測量值作為輸入，通過四元數(shù)微分方程，計(jì)算得到第一姿態(tài)四元數(shù)；

以所述第二角速度測量值作為輸入，通過四元數(shù)微分方程，計(jì)算得到第二姿態(tài)四元數(shù)。

8.根據(jù)權(quán)利要求6所述的方法，其特征在于，根據(jù)所述第一姿態(tài)四元數(shù)與所述第二姿態(tài)四元數(shù)得到第三姿態(tài)四元數(shù)的計(jì)算公式為：

其中，q_ic表示為第一姿態(tài)四元數(shù)；q_ib表示為第二姿態(tài)四元數(shù)；表示為q_ib的逆矩陣；q_bc表示為第三姿態(tài)四元數(shù)；表示四元數(shù)乘法。