[發(fā)明專利]無(wú)人機(jī)航向確定方法、裝置及無(wú)人機(jī)

有效

• 申請(qǐng)?zhí)枺?/td>: 201910765629.8
• 申請(qǐng)日: 2019-08-19
• 公開(kāi)（公告）號(hào): CN110488865B
• 公開(kāi)（公告）日: 2022-09-20
• 發(fā)明（設(shè)計(jì)）人: 李穎杰;張?zhí)肀?/a>;陳剛
• 申請(qǐng)（專利權(quán)）人: 深圳市道通智能航空技術(shù)股份有限公司
• 主分類號(hào): G05D1/10
• 分類號(hào): G05D1/10;G01C21/20;G01C21/16
• 代理公司: 深圳市六加知識(shí)產(chǎn)權(quán)代理有限公司 44372
• 代理人: 李于明

1.一種無(wú)人機(jī)航向確定方法，應(yīng)用于無(wú)人機(jī)，其特征在于，包括：

檢測(cè)所述無(wú)人機(jī)的飛行高度和所述無(wú)人機(jī)飛行環(huán)境中的磁場(chǎng)參數(shù)；

根據(jù)所述飛行高度和所述磁場(chǎng)參數(shù)，更新航向角初值；

將傳感器采集的數(shù)據(jù)與所述航向角初值進(jìn)行數(shù)據(jù)融合，得到修正后的航向角；

根據(jù)所述修正后的航向角，確定所述無(wú)人機(jī)的航向；

其中，所述根據(jù)所述飛行高度和所述磁場(chǎng)參數(shù)，更新航向角初值包括：

當(dāng)所述無(wú)人機(jī)的飛行高度小于所述飛行高度閾值時(shí)，每隔預(yù)設(shè)時(shí)間，獲取此時(shí)無(wú)人機(jī)飛行高度下的磁場(chǎng)參數(shù)；

計(jì)算每次檢測(cè)的所述磁場(chǎng)參數(shù)的誤差；

獲取檢測(cè)到的所述磁場(chǎng)參數(shù)的誤差最小時(shí)所述無(wú)人機(jī)的航向角，并將所述航向角作為航向角初值。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，

所述檢測(cè)所述無(wú)人機(jī)的飛行高度和所述無(wú)人機(jī)飛行環(huán)境中的磁場(chǎng)參數(shù)，包括：

實(shí)時(shí)檢測(cè)所述無(wú)人機(jī)的飛行高度是否小于預(yù)設(shè)的飛行高度閾值；

當(dāng)所述無(wú)人機(jī)的飛行高度小于所述飛行高度閾值時(shí)，每隔預(yù)設(shè)時(shí)間檢測(cè)一次所述無(wú)人機(jī)飛行環(huán)境中的磁場(chǎng)參數(shù)。

3.根據(jù)權(quán)利要求1所述的方法，其特征在于，

所述計(jì)算每次檢測(cè)的所述磁場(chǎng)參數(shù)的誤差，包括：

獲取所述無(wú)人機(jī)所在位置的經(jīng)緯度；

根據(jù)所述無(wú)人機(jī)所在位置的經(jīng)緯度，得到基準(zhǔn)磁場(chǎng)參數(shù)；

對(duì)比所述磁場(chǎng)參數(shù)和所述基準(zhǔn)磁場(chǎng)參數(shù)，以計(jì)算所述磁場(chǎng)參數(shù)的誤差。

4.根據(jù)權(quán)利要求2所述的方法，其特征在于，

所述磁場(chǎng)參數(shù)包括磁場(chǎng)強(qiáng)度和磁場(chǎng)傾角。

5.根據(jù)權(quán)利要求2所述的方法，其特征在于，

所述飛行高度閾值為1.5-2m。

6.根據(jù)權(quán)利要求2所述的方法，其特征在于，

所述預(yù)設(shè)時(shí)間為10-30ms。

7.根據(jù)權(quán)利要求3所述的方法，其特征在于，

所述根據(jù)所述修正后的航向角，確定所述無(wú)人機(jī)的航向，包括：

根據(jù)當(dāng)前無(wú)人機(jī)的姿態(tài)和所述修正后的航向角，更新所述無(wú)人機(jī)的姿態(tài)；

根據(jù)更新后的所述無(wú)人機(jī)的姿態(tài)，確定所述無(wú)人機(jī)的航向。

8.根據(jù)權(quán)利要求7所述的方法，其特征在于，

所述根據(jù)當(dāng)前所述無(wú)人機(jī)的姿態(tài)和所述航向角初值，更新所述無(wú)人機(jī)的姿態(tài)，包括：

獲取當(dāng)前所述無(wú)人機(jī)的姿態(tài)的四元數(shù)；

根據(jù)所述航向角初值和所述修正后的航向角，得到航向偏角；

根據(jù)所述航向偏角，得到以無(wú)人機(jī)Z軸為轉(zhuǎn)軸的四元數(shù)：

通過(guò)如下算式，確定更新后的所述無(wú)人機(jī)的姿態(tài)四元數(shù)：

q＝r*q₀；

其中，r為以所述無(wú)人機(jī)Z軸為轉(zhuǎn)軸的四元數(shù)，q₀為當(dāng)前所述無(wú)人機(jī)的姿態(tài)的四元數(shù)，q為更新后的所述無(wú)人機(jī)的姿態(tài)四元數(shù)。

9.根據(jù)權(quán)利要求8所述的方法，其特征在于，該方法還包括：通過(guò)如下算式，計(jì)算得到以所述無(wú)人機(jī)Z軸為轉(zhuǎn)軸的四元數(shù)：

其中，ψ₀為所述航向偏角，r為以所述無(wú)人機(jī)Z軸為轉(zhuǎn)軸的四元數(shù)。

10.根據(jù)權(quán)利要求1所述的方法，其特征在于，

所述檢測(cè)所述無(wú)人機(jī)的飛行高度和所述無(wú)人機(jī)飛行環(huán)境中的磁場(chǎng)參數(shù)之前，還包括：

所述無(wú)人機(jī)開(kāi)機(jī)后，初始化所述航向角初值。