用于四旋翼無人機的俯仰角的地磁修正方法
【技術領域】
[0001] 本發(fā)明設及無人機技術領域，特別設及一種用于四旋翼無人機的俯仰角的地磁修 正方法。
【背景技術】
[0002] 四旋翼無人機使用的自動駕駛儀是比較低端的，其產(chǎn)品價格較低。自動駕駛儀能 夠實現(xiàn)自動控制的基礎是姿態(tài)測量，姿態(tài)測量的基礎是巧螺儀。然而，四旋翼無人機使用的 巧螺儀由于成本限制，精度不高，再加上巧螺儀本身的漂移，在經(jīng)過長期累積后會對姿態(tài)產(chǎn) 生較大影響。

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

[0003] 本發(fā)明的目的旨在至少解決所述技術缺陷之一。
[0004] 為此，本發(fā)明的目的在于提出一種用于四旋翼無人機的俯仰角的地磁修正方法， 在沒有提高產(chǎn)品成本的前提下，實現(xiàn)了對姿態(tài)誤差的補償，提高了精度。
[0005] 為了實現(xiàn)上述目的，本發(fā)明的實施例提供一種用于四旋翼無人機的俯仰角的地磁 修正方法，包括如下步驟：
[0006]步驟S1，建立機體坐標系和導航坐標系；
[0007] 步驟S2,控制所述四旋翼無人機根據(jù)所述機體坐標系和導航坐標系W預設飛行姿 態(tài)角轉動；
[0008]步驟S3,根據(jù)所述機體轉動的飛行姿態(tài)角建立旋轉矩陣，并根據(jù)所述旋轉矩陣計 算所述四旋翼無人機的飛機姿態(tài)矩陣；
[0009] 步驟S4,設置地磁在參考系中的投影，根據(jù)所述地磁在參考系中的投影、旋轉矩陣 和飛行姿態(tài)矩陣計算地磁俯仰角修正值；
[0010] 步驟S5,根據(jù)所述地磁俯仰角修正值修正所述四旋翼無人機的俯仰角W補償所述 四旋翼無人機的巧螺儀漂移造成的姿態(tài)誤差。
[0011] 進一步，所述機體坐標系定義為：x軸表示橫滾軸，Y軸表示俯仰軸，Z軸表示航向； 所述導航坐標系定義為北東地坐標系。
[0012] 進一步，在所述步驟S2中，所述飛行姿態(tài)角為機體按照W下順序轉動時的角度： 繞所述Z軸轉動的角度Φ，繞所述Y軸轉動的角度Θ，繞所述X軸轉動的角度Φ。
[0013]進一步，在所述步驟S3中，所述旋轉矩陣為（cl，c2,c3)，
[0014] 根據(jù)繞z軸轉動Φ角，計算得到 ；
[0015] 根據(jù)繞y軸轉動Θ角，計算得到
，
[001引根據(jù)繞X軸轉動Φ角，計算得離 0
[0017] 進一步，在所述步驟S3中，根據(jù)所述旋轉矩陣計算所述四旋翼無人機的飛機姿態(tài) 矩陣，包括如下步驟：
[0018] 根據(jù)所述旋轉矩陣將參考體系變化至載體系，c"b= c3c2cl;
[0019] 然后計算所述飛機姿態(tài)矩陣Cb"為：
[0020]
[00巧]進一步，在所述步驟S4中，根據(jù)所述地磁在參考系中的投影、旋轉矩陣和飛行姿 態(tài)矩陣計算地磁俯仰角修正值Θd為：
[0026]白d=arcsin (yb/xb)，
[0027] 其中，
[0028]
[0029] 其中
趕也磁在參考系中的投影。
[0030] 進一步，所述根據(jù)地磁俯仰角修正值修正所述四旋翼無人機的俯仰角，包括如下 步驟：
[00引]Θ1=Θ1-kX(Θg-Θd)，
[0032] 其中，θ1為修正后的俯仰角，θι為上一次計算到的俯仰角θ1，θg為機體繞y 軸轉動俯仰角度，0g= (Φ，θ，4)，k為預設系數(shù)。
[0033] 進一步，k的取值范圍為0.4~0.6。
[0034] 根據(jù)本發(fā)明實施例的用于四旋翼無人機的俯仰角的地磁修正方法，通過地磁修正 俯仰角，來實現(xiàn)對巧螺儀漂移所造成的姿態(tài)誤差的誤差，W現(xiàn)有的傳感器為基礎，沒有增加 額外的傳感器，從而在沒有提高產(chǎn)品成本的前提下，實現(xiàn)了對姿態(tài)誤差的補償，提高了精 度。
[0035] 本發(fā)明附加的方面和優(yōu)點將在下面的描述中部分給出，部分將從下面的描述中變 得明顯，或通過本發(fā)明的實踐了解到。
【附圖說明】
[0036] 本發(fā)明的上述和/或附加的方面和優(yōu)點從結合下面附圖對實施例的描述中將變 得明顯和容易理解，其中：
[0037] 圖1為根據(jù)本發(fā)明實施例的用于四旋翼無人機的俯仰角的地磁修正方法的流程 圖。
【具體實施方式】
[0038] 下面詳細描述本發(fā)明的實施例，所述實施例的示例在附圖中示出，其中自始至終 相同或類似的標號表示相同或類似的元件或具有相同或類似功能的元件。下面通過參考附 圖描述的實施例是示例性的，旨在用于解釋本發(fā)明，而不能理解為對本發(fā)明的限制。
[003引如圖1所示，本發(fā)明實施例的用于四旋翼無人機的俯仰角的地磁修正方法，包括 如下步驟：
[0040] 步驟S1，建立機體坐標系和導航坐標系。
[0041] 具體地，坐標系建立如下：機體坐標系定義為：X軸表示橫滾軸，對應前方；Y軸表 示俯仰軸，對應右方；Z軸表示航向，對應下方。導航坐標系定義為北東地坐標系。
[0042] 步驟S2,控制四旋翼無人機根據(jù)機體坐標系和導航坐標系W預設飛行姿態(tài)角轉 動。
[0043] 飛行姿態(tài)角為機體按照W下順序轉動時的角度：繞Z軸轉動的角度Φ，繞Y軸轉 動的角度Θ，繞X軸轉動的角度Φ。
[0044] 步驟S3,根據(jù)機體轉動的飛行姿態(tài)角建立旋轉矩陣，并根據(jù)旋轉矩陣計算四旋翼 無人機的飛機姿態(tài)矩陣。
[0045] 機體轉動按照繞航向軸狂軸）轉動Φ，俯仰軸燈軸）轉動Θ，橫滾軸狂軸）轉 動Φ的順序轉動時，旋轉矩陣為（cl，c2,c3)，
[0046] 根據(jù)繞z軸轉動Φ角，計算得離
[0047] 根據(jù)繞y軸轉動θ角，計算得到
;(2)
[0048] 根據(jù)繞X軸轉動Φ角，計算得至 (3)
[0049] 基于此，根據(jù)旋轉矩陣計算四旋翼無人機的飛機姿態(tài)矩陣，包括如下步驟：
[0050]首先，根據(jù)旋轉矩陣將參考體系變化至載體系，
[0051]c3c2cl；(4)
[0052] 然后，計算飛機姿態(tài)矩陣C為：
[00巧]進一步，可W根據(jù)上述飛機姿態(tài)矩陣計算飛機姿態(tài)角為：
[0059] 步驟S4,設置地磁在參考系中的投影，根據(jù)地磁在參考系中的投影、旋轉矩陣和飛 行姿態(tài)矩陣計算地磁俯仰角修正值Θd。
[0060] 根據(jù)地磁在參考系中的投影、旋轉矩陣和飛行姿態(tài)矩陣計算地磁俯仰角修正值 白d。
[0061] 白d=arcsin(yb/xb)，
[006引其中，
[0063]
[0064] 其中：
趕也磁在參考系中的投影。
[0065] 步驟S5,根據(jù)地磁俯仰角修正值修正四旋翼無人機的俯仰角W補償四旋翼無人機 的巧螺儀漂移造成的姿態(tài)誤差。
[0066]Θ1=Θi-kX(Θg-Θd)，
[0067]其中，θ1為修正后的俯仰角，θι為上一次計算到的俯仰角θ1，θg為機體繞y 軸轉動俯仰角度，0g= (Φ，θ，4)，k為預設系數(shù)。k的取值范圍為0.4~0.6。
[0068] 根據(jù)本發(fā)明實施例的用于四旋翼無人機的俯仰角的地磁修正方法，通過地磁修正 俯仰角，來實現(xiàn)對巧螺儀漂移所造成的姿態(tài)誤差的誤差，W現(xiàn)有的傳感器為基礎，沒有增加 額外的傳感器，從而在沒有提高產(chǎn)品成本的前提下，實現(xiàn)了對姿態(tài)誤差的補償，提高了精 度。
[0069] 在本說明書的描述中，參考術語"一個實施例"、"一些實施例"、"示例"、"具體示 例"、或"一些示例"等的描述意指結合該實施例或示例描述的具體特征、結構、材料或者特 點包含于本發(fā)明的至少一個實施例或示例中。在本說明書中，對上述術語的示意性表述不 一定指的是相同的實施例或示例。而且，描述的具體特征、結構、材料或者特點可W在任何 的一個或多個實施例或示例中W合適的方式結合。
[0070] 盡管上面已經(jīng)示出和描述了本發(fā)明的實施例，可W理解的是，上述實施例是示例 性的，不能理解為對本發(fā)明的限制，本領域的普通技術人員在不脫離本發(fā)明的原理和宗旨 的情況下在本發(fā)明的范圍內(nèi)可W對上述實施例進行變化、修改、替換和變型。本發(fā)明的范圍 由所附權利要求極其等同限定。
【主權項】
1. 一種用于四旋翼無人機的俯仰角的地磁修正方法，其特征在于，包括如下步驟： 步驟S1，建立機體坐標系和導航坐標系； 步驟S2,控制所述四旋翼無人機根據(jù)所述機體坐標系和導航坐標系W預設飛行姿態(tài)角 轉動； 步驟S3,根據(jù)所述機體轉動的飛行姿態(tài)角建立旋轉矩陣，并根據(jù)所述旋轉矩陣計算所 述四旋翼無人機的飛機姿態(tài)矩陣； 步驟S4,設置地磁在參考系中的投影，根據(jù)所述地磁在參考系中的投影、旋轉矩陣和飛 行姿態(tài)矩陣計算地磁俯仰角修正值； 步驟S5,根據(jù)所述地磁俯仰角修正值修正所述四旋翼無人機的俯仰角W補償所述四旋 翼無人機的巧螺儀漂移造成的姿態(tài)誤差。2. 如權利要求1所述的用于四旋翼無人機的俯仰角的地磁修正方法，其特征在于，所 述機體坐標系定義為：X軸表示橫滾軸，Y軸表示俯仰軸，Z軸表示航向；所述導航坐標系定 義為北東地坐標系。3. 如權利要求1所述的用于四旋翼無人機的俯仰角的地磁修正方法，其特征在于，在 所述步驟S2中，所述飛行姿態(tài)角為機體按照W下順序轉動時的角度：繞所述Z軸轉動的角 度Φ，繞所述Y軸轉動的角度Θ，繞所述X軸轉動的角度Φ。4. 如權利要求1所述的用于四旋翼無人機的俯仰角的地磁修正方法，其特征在于，在 所述步驟S3中，所述旋轉矩陣為（cl，c2,c3)，根據(jù)繞z軸轉動Φ角，計算得到根據(jù)繞y軸轉動Θ角，計算得質 ；根據(jù)繞X軸轉動Φ角，計算得到 〇5. 如權利要求4所述的用于四旋翼無人機的俯仰角的地磁修正方法，其特征在于，在 所述步驟S3中，根據(jù)所述旋轉矩陣計算所述四旋翼無人機的飛機姿態(tài)矩陣，包括如下步 驟： 根據(jù)所述旋轉矩陣將參考體系變化至載體系，c"b=c3c2cl; 然后計算所述飛機姿態(tài)矩陣Cb"為：6. 如權利要求5所述的用于四旋翼無人機的俯仰角的地磁修正方法，其特征在于，在 所述步驟S4中，根據(jù)所述地磁在參考系中的投影、旋轉矩陣和飛行姿態(tài)矩陣計算地磁俯仰 角修正值Θd為： 白d=arcsin(yb/xb)， 其中，其中為地磁在參考系中的投影。7. 如權利要求6所述的用于四旋翼無人機的俯仰角的地磁修正方法，其特征在于，在 所述步驟S5中，所述根據(jù)地磁俯仰角修正值修正所述四旋翼無人機的俯仰角，包括如下步 驟： 白1 =白_i_kX(白邑一白d), 其中，Θ1為修正后的俯仰角，θι為上一次計算到的俯仰角Θ1，0g為機體繞y軸轉 動俯仰角度，0g= (Φ，Θ，il〇，k為預設系數(shù)。8. 如權利要求7所述的用于四旋翼無人機的俯仰角的地磁修正方法，其特征在于，k的 取值范圍為0.4~0.6。
【專利摘要】本發(fā)明提出了一種用于四旋翼無人機的俯仰角的地磁修正方法，包括：建立機體坐標系和導航坐標系；控制四旋翼無人機根據(jù)機體坐標系和導航坐標系以預設飛行姿態(tài)角轉動；根據(jù)機體轉動的飛行姿態(tài)角建立旋轉矩陣，并根據(jù)旋轉矩陣計算四旋翼無人機的飛機姿態(tài)矩陣；設置地磁在參考系中的投影，根據(jù)地磁在參考系中的投影、旋轉矩陣和飛行姿態(tài)矩陣計算地磁俯仰角修正值；根據(jù)地磁俯仰角修正值修正四旋翼無人機的俯仰角以補償四旋翼無人機的陀螺儀漂移造成的姿態(tài)誤差。本發(fā)明在沒有提高產(chǎn)品成本的前提下，實現(xiàn)了對姿態(tài)誤差的補償，提高了精度。
【IPC分類】G01C21/06, G01C21/18, G01C21/08
【公開號】CN105403218
【申請?zhí)枴緾N201510888686
【發(fā)明人】黃磊, 王根勇, 劉聰, 李學風
【申請人】北京健德乾坤導航系統(tǒng)科技有限責任公司
【公開日】2016年3月16日
【申請日】2015年12月8日