用于四旋翼飛行器的控制方法
【專利摘要】本發(fā)明公開了一種對四旋翼飛行器的姿態(tài)角和位置均能有效、快速地控制的用于四旋翼飛行器的控制方法。該用于四旋翼飛行器的控制方法通過利用INS慣性導航和GPS導航組合導航的方法解決了單一的GPS導航技術易受干擾和遮擋，短時定位精度不高，輸出頻率有限并且輸出不連續(xù)的缺點；可以獲得四旋翼飛行器較為準確的導航信息參數(shù)，接著利用準確的導航信息參數(shù)計算得到四旋翼飛行器的控制輸入量；進而將其輸入四旋翼飛行器的控制系統(tǒng)進行飛行控制，該控制方法對導航信息參數(shù)進行優(yōu)化，可以得到較為準確的導航信息參數(shù)，同時對控制算法進行優(yōu)化，可以使四旋翼飛行器的姿態(tài)角和位置均能有效、快速地控制。適合在飛行控制領域推廣應用。
【專利說明】
用于四旋翼飛行器的控制方法
技術領域
[0001] 本發(fā)明涉及飛行控制領域，尤其是一種用于四旋翼飛行器的控制方法。
【背景技術】
[0002] 由于四旋翼具有好的機動性能、簡潔的結(jié)構(gòu)和穩(wěn)定的飛行能力，使得它在許多的 場合，如環(huán)境保護、質(zhì)地勘測、農(nóng)業(yè)管理，電力巡線和電影工業(yè)等領域發(fā)揮了重要作用。然 而，四旋翼飛行器是一個非線性、欠驅(qū)動、高耦合的控制系統(tǒng)，因此要獲得準確的數(shù)學模型 和好的控制算法并不容易。國內(nèi)外的許多研究人員對四旋翼的數(shù)學模型，導航技術和控制 方法進行了研究，并取得了許多重要成果。
[0003] 在飛行控制算法方面，由于四旋翼是一個非線性系統(tǒng)，因此對四旋翼的控制有兩 個大的方向，一種是直接將系統(tǒng)按非線性處理，得到非線性控制算法;另一種是將非線性系 統(tǒng)線性化得到線性的系統(tǒng)表達式，然后運用線性系統(tǒng)理論的相關知識來獲得控制算法。非 線性控制算法計算比較復雜，但控制效果準確。
[0004] 四旋翼飛行器是由兩個相互垂直的剛性結(jié)構(gòu)體構(gòu)成十字型的機架，在四個角上分 別安裝上直流電機。四旋翼在空間有六個自由度，可以實現(xiàn)六種基本的運動:沿著z軸的上 下運動，沿著x軸的前后運動，沿著y軸的左右運動;繞著x軸的滾轉(zhuǎn)運動，繞著y軸的俯仰運 動，繞著z軸的偏航運動。同時增加或減少四個電機的推力，可以實現(xiàn)上下運動;通過不同的 推力控制策略可以實現(xiàn)前后、左右和偏航運動。四旋翼在三維空間內(nèi)的任意一種復雜運動 都可以看作是這六種基本運動的組合。因此，對四旋翼的控制實際上就是對這六個自由度 的控制，其數(shù)學模型也是以這六個自由度為參數(shù)的。
[0005] 對四旋翼的飛行控制可以分為姿態(tài)控制和位置控制。姿態(tài)控制可以保證四旋翼在 飛行時能實時控制飛機的姿態(tài)以防止墜機;位置控制可以保證飛機可以向任何方向飛行。 但四旋翼強耦合、非線性、欠驅(qū)動、干擾敏感的特性，使得飛行控制系統(tǒng)的設計變得非常困 難。此外，控制系統(tǒng)的性能還受到模型準確性和傳感器精度的影響。雖然四旋翼直升機控制 系統(tǒng)的設計面臨著種種困難，但其獨特的性能優(yōu)勢和廣泛的應用領域吸引著越來越多的學 者對其進行研究。
[0006] 四旋翼的飛行控制的基礎在于導航信息參數(shù)的準確性，獲得的導航信息參數(shù)越準 確，其飛行控制越準確。在導航技術方面，目前應用得最多，最成熟的導航方式有INS慣性導 航和衛(wèi)星導航。GPS衛(wèi)星導航的優(yōu)點是具有全球性、全天候、長時間定位精度高的特點，但缺 點是信號易受干擾和遮擋，在強電磁環(huán)境下和有高樓遮擋時，信號質(zhì)量變差，并且其輸出頻 率有限，一般為1 一 10Hz，輸出不連續(xù)，在需要快速更新信息的場合，如機動性和實時性要求 較高的無人機系統(tǒng)上，GPS衛(wèi)星導航的缺點便凸顯出來。而INS慣性導航系統(tǒng)是一種全自主 式的導航方式，因此具有很強的隱蔽性和抗干擾的能力，并且輸出信息連續(xù)，短時間內(nèi)定位 精度高。但由于MEMS-INS器件自身的特點，陀螺儀和加速度計有初始零偏、隨機漂移等誤 差，隨著時間的累計作用，其誤差越來越大，長時間定位精度較差，最終無法準確反映無人 機的姿態(tài)和位置信息。
【發(fā)明內(nèi)容】
[0007] 本發(fā)明所要解決的技術問題是提供一種對四旋翼飛行器的姿態(tài)角和位置均能有 效、快速地控制的用于四旋翼飛行器的控制方法。
[0008] 本發(fā)明解決其技術問題所采用的技術方案為:該用于四旋翼飛行器的控制方法， 包括以下步驟：
[0009] S1、利用INS慣性導航與GPS導航以及磁力計的導航算法計算得到四旋翼飛行器的 導航信息參數(shù)；
[0010] S2、將步驟S1得到的導航信息參數(shù)代入如下公式得到四旋翼飛行器的控制輸入量 UhUhUhUh
[0015]其中，叉1=丫，4=；?> =為，叉3 = 0，.'4=6 = 乂.;，.》5=浐％6..=_分=、.9是俯仰角，丫是滾 轉(zhuǎn)角，供是偏航角，?為轉(zhuǎn)速擾動，m為四旋翼飛行器的質(zhì)量，g為重力加速度，ahahahazt、 a5、a6、a7、a8為大于零的正數(shù)
，1表示四旋翼飛行器的機臂長度，Ixx，Iyy，Izz 分別是四旋翼飛行器繞x軸，y軸，z軸的轉(zhuǎn)動慣量，Jr為四旋翼飛行器的電機轉(zhuǎn)子的轉(zhuǎn)動慣 量，21是俯仰角的目標值和通過步驟S1計算得到實際值之差，z2是俯仰角速度目標值和通過 步驟S1計算得到實際值之差; Z3是滾轉(zhuǎn)角的目標值和通過步驟S1計算得到實際值之差，Z4是 滾轉(zhuǎn)角速度目標值和通過步驟S1計算得到實際值之差; 25是偏航角的目標值和通過步驟S1 計算得到實際值之差，Z6是偏航角速度目標值和通過步驟S1計算得到實際值之差;27是冗軸 高度的目標值和通過步驟S1計算得到實際值之差，Z8是Z軸高度方向上速度目標值和通過 步驟S1計算得到實際值之差；
[0016] S3、將步驟S2計算得到的四旋翼飛行器的控制輸入量^、^、^、山輸入四旋翼飛行 器的控制系統(tǒng)進行飛行控制。
[0017] 進一步的是，在步驟S1中，所述INS慣性導航與GPS導航以及磁力計的導航算法的 具體計算方法如下所述：
[0018] A、將INS慣性導航系統(tǒng)的陀螺儀測得的四旋翼飛行器角速度參數(shù) 代入四元數(shù)微分方程求解得到四元數(shù)9〇^142^3;其中64：*?4.，< 6,為陀螺儀在四旋翼 飛行器自身坐標系下的測得的三個軸的角速度信息；
[0019]所述四元數(shù)微分方程為：
[0021] B、將步驟A中求解的(^，(^，(^，(^代入下式求解得到姿態(tài)矩陣^^， %+9i+%+% 2(^2+^) 2(q^-q{)q2)
[0022] liq^-cf^q,) q,-(f,+(f2-qs 2(q1q,+(f i)q]) _2(^<7,+a0a,) 2(n 糾丨）an + %_
[0023] 根據(jù)下述與方向余弦的關系式 coi^a^sr -sin^;a^/+cosr/;sin^sin7 .-cos你 in;/
[0024] = sin^cos^ ccstpcosO sin 沒 cos^?sin,_sin妒sin沒oosf -sin^sinf-cosf?sin^cosf cm沒cos,
[0025] 計算得出四旋翼飛行器的INS慣性導航模塊姿態(tài)角0、Y、f&
[0026] C、利用磁力計測得的偏航角#替換步驟B計算得到的偏航角
[0027] D、將INS慣性導航系統(tǒng)的加速度計測得的加速度參數(shù)護和步驟B中求解得到姿態(tài) 矩陣g代入下述微分方程中求解得到四旋翼飛行器在INS慣性導航坐標系下的東、北、天三 個方向上的速度信息VN VE VU，所述微分方程為：
[0028] =： df ~ (2< + < ) X V" +
[0029] 其中，vn=[VN vE vu]'分別為INS慣性導航坐標系中東、北、天方向上的速度，fiC為 地球自轉(zhuǎn)角速度，為四旋翼飛行器繞INS慣性導航坐標系各軸向的轉(zhuǎn)動角速率，g n為重力 加速度；
[0030] E、將步驟D計算得出的VN VE VU分別代入下式求解得出四旋翼飛行器在INS慣性導 航系統(tǒng)中的位置信息，其中L為煒度，A為經(jīng)度，h為高度，
h = h(0)+/vudt,其中L(0)表示四旋翼飛行 7 器初始位置的煒度值A(0)表示四旋翼飛行器初始位置的經(jīng)度值，h(0)表示四旋翼飛行器 距離地球表面的初始高度。Rm表示地球子午圈上的曲率半徑，Rn表示煒度圈上的曲率半徑； [0032] F、建立狀態(tài)方程夂（/) =巧（/) .Y, (/) + G, (、/)%(/)和觀測方程Z(t) ⑴，
[0033] X〗（t)表示INS慣性導航系統(tǒng)在t時刻的誤差狀態(tài)，它是一個15維的向量，如下所 示：
[0034] X(V)=[加 T 5vr 私戎為.於況況激《6 ▽r]，5vx，5Vy， 5VzS INS慣性導航系統(tǒng)沿東、北、天方向上的速度誤差；(K，(i>y，(K為四旋翼飛行器的姿態(tài)角誤 差;SL，SA，Sh分別代表四旋翼飛行器所在煒度、經(jīng)度和高度誤差；^，^，^分別代表陀螺儀 的隨機漂移；u,.，v;分別為加速度計的隨機漂移，其中匕⑴=，是一 L 0 fm (0J15x15 個15X15的矩陣;其中FN(t)對應于5%5%，^，(}^，<^，4)2，乩，從，511這9個參數(shù)的1吧慣 性導航系統(tǒng)矩陣，其非零元素如下：
[0048]卩5(1:)為5~，^，^，（^，<^，傘2,81，5入，5]1這9個參數(shù)與陀螺儀及加速度計漂移 _u_ 之間的變換矩陣，其維數(shù)是9X6,巧(〇= C； 03x3 ; _〇3x3 〇3x3 _
[0049] FM(t)為￡\6，￡2，^入與陀螺儀及加速度計漂移對應的1剌貫性導航系統(tǒng)矩 陣，是一個維數(shù)為6 X 6的對角線矩陣，表示如下：
[0050] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中，Tgx表示陀螺儀x 軸的誤差模型的時間常數(shù)，Tgy表示陀螺儀y軸的誤差模型的時間常數(shù)，Tgz表示陀螺儀z軸的 誤差模型的時間常數(shù)，T ax表示加速度計x軸誤差模型的時間常數(shù)，Tay表示加速度計y軸誤差 模型的時間常數(shù)，T az表示加速度計z軸誤差模型的時間常數(shù)；
[0051] Gi(t) = diag[l 1 1......11] 15X15 ；
[0052] Wi(t)是一個15維的向量，如下所示：
[0053] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5]，
[0054] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系統(tǒng)過程噪聲序列；
[0055]
[0056] Z(t)為四旋翼飛行器在INS慣性導航中的位置速度信息與四旋翼飛行器在GPS導 航系統(tǒng)中的位置速度信息的差值，是一個6維向量，
[0057] Z(t) = [5vx+Nvx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5人+NX 5h+Nh]T，其中，Nvx 表示GPS導航系統(tǒng)在x方向上的速度誤差，Nvy表示GPS導航系統(tǒng)在y方向上的速度誤差，Nvz表 示GPS導航系統(tǒng)在z方向上的速度誤差，N x表示GPS導航系統(tǒng)在x方向上的位置誤差，Ny表示 GPS導航系統(tǒng)在y方向上的位置誤差，Nh表示GPS導航系統(tǒng)在z方向上的位置誤差；
[Goss]
[0059] IIt(l) = \diag[\ 1 l]：〇；xl:j
[0060] g[(i?A/+/?) (/?'+/?)cosI
[0061] Vv(t) = [Nvx Nvy Nvz]t
[0062] VP(t) = [Nx Ny Nz]t
[0063] G、將上述得到的連續(xù)狀態(tài)方程尤（/) = A G) + G/ G)離散化后得到知= ① k, k-iXk-i+Wk-1，其中
[0064] 將上述得到的連續(xù)觀測方程ZUhmOXKtHVU)離散化后得到Zk = HkXk+Vk;
[0065] 其中I是單位矩陣，F(xiàn)是INS慣性導航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，A t是離散化后INS慣性 導航系統(tǒng)的采樣時間；
[0066] H、將四旋翼飛行器在INS慣性導航系統(tǒng)中的位置速度信息與四旋翼飛行器在GPS 導航系統(tǒng)中的位置速度信息作差得到Z(t)在k時刻的觀測信息z;
[0067] I、計算k時刻INS慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值之降.，童心.=_氧|卜.丨+.& (z -名.卜j), 其中 尤Hi為在k -1時刻IN S慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值， A 對(44-此+41，心―,+這-tA-1是INS慣性導航系統(tǒng)的噪聲矩陣，其 ~ H 1 大小是由INS慣性導航元件的性能決定，A =好，& = \1 一KkH-KkHJ十[々篇， Rk是系統(tǒng)測量噪聲的方差陣，其大小是由GPS接收機的性能決定；
[0068] J、將計算得到的值與四旋翼飛行器在INS慣性導航系統(tǒng)中的位置速度信息作 差得到最優(yōu)的導航參數(shù)；
[0069] K、重復步驟H-J，得到連續(xù)的四旋翼飛行器的導航信息參數(shù)。
[0070] 本發(fā)明的有益效果:該用于四旋翼飛行器的控制方法通過利用INS慣性導航和GPS 導航組合導航的方法解決了單一的GPS導航技術易受干擾和遮擋，短時定位精度不高，輸出 頻率有限并且輸出不連續(xù)的缺點；同時也解決了單一的INS慣性導航參數(shù)累計誤差越來越 大，長時間定位精度發(fā)散的缺點，利用GPS導航長時間具有高的定位精度的優(yōu)點來彌補INS 慣性導航累計誤差隨時間的增加而發(fā)散的缺點；利用INS慣性導航不受外界干擾、輸出的導 航信息連續(xù)的特點彌補GPS易受干擾和輸出頻率有限的缺點，并且為了解決由慣性導航計 算出的偏航角無法找到真北，以及漂移較大的情況，本系統(tǒng)利用磁力計計算出的偏航角來 校正，獲得地理真北方向和穩(wěn)定的偏航角，可以獲得四旋翼飛行器較為準確的導航信息參 數(shù)，接著利用準確的導航信息參數(shù)計算得到四旋翼飛行器的控制輸入量;進而將其輸入四 旋翼飛行器的控制系統(tǒng)進行飛行控制，該控制方法對導航信息參數(shù)進行優(yōu)化，可以得到較 為準確的導航信息參數(shù)，同時對控制算法進行優(yōu)化，可以使四旋翼飛行器的姿態(tài)角和位置 均能有效、快速地控制。
【附圖說明】
[0071] 圖1為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的煒度 誤差值；
[0072]圖2為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的煒度 誤差方差值；
[0073]圖3為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的經(jīng)度 誤差值；
[0074]圖4為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的經(jīng)度 誤差方差值；
[0075]圖5為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的高度 誤差值；
[0076]圖6為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的高度 誤差方差值；
[0077]圖7為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的東向 速度誤差值；
[0078]圖8為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的東向 速度誤差方差值；
[0079]圖9為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的北向 速度誤差值；
[0080] 圖10為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的北 向速度誤差方差值；
[0081] 圖11為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的天 向速度誤差值；
[0082]圖12為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的天 向速度誤差方差值；
[0083]圖13為滾轉(zhuǎn)角的仿真效果圖；
[0084]圖14為俯仰角的仿真效果圖；
[0085]圖15為偏航角的仿真效果圖；
[0086]圖16為四旋翼飛行器在x軸，y軸和z軸的位置仿真圖；
[0087]圖17為四旋翼飛行器在三維空間中的軌跡圖；
[0088] 圖18為四旋翼飛行器的飛行軌跡圖。
【具體實施方式】
[0089] 本發(fā)明所述的用于四旋翼飛行器的控制方法，包括以下步驟：
[0090] S1、利用INS慣性導航與GPS導航以及磁力計的導航算法計算得到四旋翼飛行器的 導航信息參數(shù)；
[0091] S2、將步驟S1得到的導航信息參數(shù)代入如下公式得到四旋翼飛行器的控制輸入量 UhUhUhUh
[0096] 其中，xi= y，x2.二戶=夫?叉3 = 0，；《4 =6.= i3.，=廬 x6 =分'=毛..，.0 是俯仰角，y 是滾 轉(zhuǎn)角，口是偏航角，為轉(zhuǎn)速擾動，m為四旋翼飛行器的質(zhì)量，g為重力加速度，(^、(^、(^、(^、 a5、a6、a7、a8為大于零的正數(shù)：
，1表示四旋翼飛行器的機臂長度，Ixx，Iyy，Izz 分別是四旋翼飛行器繞x軸，y軸，z軸的轉(zhuǎn)動慣量，Jr為四旋翼飛行器的電機轉(zhuǎn)子的轉(zhuǎn)動慣 量，21是俯仰角的目標值和通過步驟S1計算得到實際值之差，z2是俯仰角速度目標值和通過 步驟S1計算得到實際值之差; Z3是滾轉(zhuǎn)角的目標值和通過步驟S1計算得到實際值之差，Z4是 滾轉(zhuǎn)角速度目標值和通過步驟S1計算得到實際值之差; 25是偏航角的目標值和通過步驟S1 計算得到實際值之差，Z6是偏航角速度目標值和通過步驟S1計算得到實際值之差;27是冗軸 高度的目標值和通過步驟S1計算得到實際值之差，Z8是Z軸高度方向上速度目標值和通過 步驟S1計算得到實際值之差；
[0097] S3、將步驟S2計算得到的四旋翼飛行器的控制輸入量^、^、^、山輸入四旋翼飛行 器的控制系統(tǒng)進行飛行控制。
[0098] 該用于四旋翼飛行器的控制方法通過利用INS慣性導航和GPS導航組合導航的方 法解決了單一的GPS導航技術易受干擾和遮擋，短時定位精度不高，輸出頻率有限并且輸出 不連續(xù)的缺點；同時也解決了單一的INS慣性導航參數(shù)累計誤差越來越大，長時間定位精度 發(fā)散的缺點，利用GPS導航長時間具有高的定位精度的優(yōu)點來彌補INS慣性導航累計誤差隨 時間的增加而發(fā)散的缺點;利用INS慣性導航不受外界干擾、輸出的導航信息連續(xù)的特點彌 補GPS易受干擾和輸出頻率有限的缺點，并且為了解決由慣性導航計算出的偏航角無法找 到真北，以及漂移較大的情況，本系統(tǒng)利用磁力計計算出的偏航角來校正，獲得地理真北方 向和穩(wěn)定的偏航角，可以獲得四旋翼飛行器較為準確的導航信息參數(shù)，接著利用準確的導 航信息參數(shù)計算得到四旋翼飛行器的控制輸入量;進而將其輸入四旋翼飛行器的控制系統(tǒng) 進行飛行控制，該控制方法對導航信息參數(shù)進行優(yōu)化，可以得到較為準確的導航信息參數(shù)， 同時對控制算法進行優(yōu)化，可以使四旋翼飛行器的姿態(tài)角和位置均能有效、快速地控制。
[0099] 所述控制中心采用如下方法計算得到最終的導航信息，其具體計算方法如下所 述：
[0100] 在步驟S1中，所述INS慣性導航與GPS導航以及磁力計的導航算法的具體計算方法 如下所述：
[0101] A、將INS慣性導航系統(tǒng)的陀螺儀測得的四旋翼飛行器角速度參數(shù)乂fa， 代入四元數(shù)微分方程求解得到四元數(shù)9〇^142^3;其中?^，64/?4 2為陀螺儀在四旋翼 飛行器自身坐標系下的測得的三個軸的角速度信息；
[0102] 所述四元數(shù)微分方程為：
[0104] B、將步驟A中求解的(^，(^(^，陽代入下式求解得到姿態(tài)矩陣^， 2(秘 +祕）2(_-跡）
[0105] Cl = 2(qlq2 -qQq3) 免 2(q2q3 +q0ql) _2(q]q,+q{)q2) 2(c/nq') q0-q, -q2 +q,_
[0106] 根據(jù)下述Cf與方向余弦的關系式 cos (poos y -sin (pccBy+cos c^sin ^sin/ -a?6^sin/
[oi07] sirupccBO ccs(pc(B〇 %mO _cos^sin;/-sin^sin^oosf -siripsiiif-cos穸sin汐cos/ cos^GOSf _
[0108]計算得出四旋翼飛行器的INS慣性導航模塊姿態(tài)角0、y、P;
[0109] C、利用磁力計測得的偏航角※替換步驟B計算得到的偏航角 [011 0] D、將INS慣性導航系統(tǒng)的加速度計測得的加速度參數(shù)滬和步驟B中求解得到姿態(tài) 矩陣g代入下述微分方程中求解得到四旋翼飛行器在INS慣性導航坐標系下的東、北、天三 個方向上的速度信息VN VE VU，所述微分方程為：
[cm] v?=C；//7-(2< + <)xvw+gH
[0112] 其中，vn=[VN VE VU]'分別為INS慣性導航坐標系中東、北、天方向上的速度，<為 地球自轉(zhuǎn)角速度，< 為四旋翼飛行器繞INS慣性導航坐標系各軸向的轉(zhuǎn)動角速率，gn為重力 加速度；
[0113] E、將步驟D計算得出的VN VE vu分別代入下式求解得出四旋翼飛行器在INS慣性導 航系統(tǒng)中的位置信息，其中L為煒度，A為經(jīng)度，h為高度，
h = h(0)+/vudt,其中L(0)表示四旋翼飛行 '， 器初始位置的煒度值A(0)表示四旋翼飛行器初始位置的經(jīng)度值，h(0)表示四旋翼飛行器 距離地球表面的初始高度。Rm表示地球子午圈上的曲率半徑，Rn表示煒度圈上的曲率半徑；
[0115] F、建立狀態(tài)方程夂（/) = Z7, (/)丨（/) + 6 (/) % (/)和觀測方程Z (t) = H( t )X〗（t) +V (t)，
[0116] XKt)表示INS慣性導航系統(tǒng)在t時刻的誤差狀態(tài)，它是一個15維的向量，如下所 示：
[0117] 本⑷=[火況，，與我為為級說廉q $ & % Vz]，SVx，SVy，SVz* INS慣性導航系統(tǒng)沿東、北、天方向上的速度誤差；(K，(i>y，為四旋翼飛行器的姿態(tài)角誤 差;SL，SA，Sh分別代表四旋翼飛行器所在煒度、經(jīng)度和高度誤差；^，^，^分別代表陀螺儀 的隨機漂移^乂^分別為加速度計的隨機漂移^中朽^7^^* ，^ ，是一 L 0 & (，)L 個15X15的矩陣;其中FN(t)對應于5%5%，^，(}^，<^，4)2，乩，從，511這9個參數(shù)的1吧慣 性導航系統(tǒng)矩陣，其非零元素如下：
[0131] 卩5(1:)為5~，5%，^，(^，<^，巾2,81，5人，5]1這9個參數(shù)與陀螺儀及加速度計漂移 & 之間的變換矩陣，其維數(shù)是9X6，K(〇= q 03x3 ; _〇3x3 〇.3.x3
[0132] Fm( t)為ex，ey，ez，▽、，▽,.▽:與陀螺儀及加速度計漂移對應的INS慣性導航系統(tǒng)矩 陣，是一個維數(shù)為6 X 6的對角線矩陣，表示如下：
[0133] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中，Tgx表示陀螺儀x 軸的誤差模型的時間常數(shù)，Tgy表示陀螺儀y軸的誤差模型的時間常數(shù)，Tgz表示陀螺儀z軸的 誤差模型的時間常數(shù)，T ax表示加速度計x軸誤差模型的時間常數(shù)，Tay表示加速度計y軸誤差 模型的時間常數(shù)，T az表示加速度計z軸誤差模型的時間常數(shù)；
[0134] Gi(t) = diag[l 1 1......1 I]i5xi5；
[0135] Wi(t)是一個15維的向量，如下所示：
[0136] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5]，
[0137] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系統(tǒng)過程噪聲序列；
[0138] Z(t)為四旋翼飛行器在INS慣性導航中的位置速度信息與四旋翼飛行器在GPS導 航系統(tǒng)中的位置速度信息的差值，是一個6維向量，
[0139] Z(t) = [5vx+Nvx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5入+NX 5h+Nh]T，其中，Nvx 表示GPS導航系統(tǒng)在x方向上的速度誤差，Nvy表示GPS導航系統(tǒng)在y方向上的速度誤差，Nvz表 示GPS導航系統(tǒng)在z方向上的速度誤差，N x表示GPS導航系統(tǒng)在x方向上的位置誤差，Ny表示 GPS導航系統(tǒng)在y方向上的位置誤差，Nh表示GPS導航系統(tǒng)在z方向上的位置誤差； _]剛m '其中
[0141] ll,U)^\diag[\ 1 1 ]!〇,,,：}
[0142] //J/) = [(/?,,, +/;) (R, +/;)cosL l]:03x6}
[0143] Vv(t) = [Nvx Nvy Nvz]t
[0144] VP(t) = [Nx Ny Nz]t
[0145] G、將上述得到的連續(xù)狀態(tài)方程= + 離散化后得到Xk = ① k, k-lXk-l+Wk-1，其中
[0146] 將上述得到的連續(xù)觀測方程Z(t)=H(t)XI(t)+V(t)離散化后得到Zk = HkXk+Vk;
[0147] 其中I是單位矩陣，F(xiàn)是INS慣性導航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，A t是離散化后INS慣性 導航系統(tǒng)的采樣時間；
[0148] H、將四旋翼飛行器在INS慣性導航系統(tǒng)中的位置速度信息與四旋翼飛行器在GPS 導航系統(tǒng)中的位置速度信息作差得到Z(t)在k時刻的觀測信息z;
[0149] I、計算k時刻INS慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值之力，名^-=之|卜丨-小 其中，矣h d為在k _ 1時刻IN S慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值， A = VXbVM+&r，1是頂S慣性導航系統(tǒng)的噪聲矩陣， 其大小是由INS慣性導航元件的性能決定，札=^Pk = [l-KLHk]PLkA [l-KkHkf +K,RkK[, Rk是系統(tǒng)測量噪聲的方差陣，其大小是由GPS接收機的性能決定；
[0150] J、將計算得到的之丨值與四旋翼飛行器在INS慣性導航系統(tǒng)中的位置速度信息作 差得到最優(yōu)的導航參數(shù)；
[0151] K、重復步驟H-J，得到連續(xù)的四旋翼飛行器的導航信息參數(shù)。
[0152] 利用上述方法計算得出的導航信息，能夠準確反映四旋翼飛行器的姿態(tài)和位置信 息，可以實現(xiàn)較好的定位導航精度，圖1為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的 導航算法計算得出的煒度誤差值；圖2為本發(fā)明所述INS慣性導航與GPS導航以及磁力計的 導航算法計算得出的煒度誤差方差值；圖3為本發(fā)明所述INS慣性導航與GPS導航以及磁力 計的導航算法計算得出的經(jīng)度誤差值；圖4為本發(fā)明所述INS慣性導航與GPS導航以及磁力 計的導航算法計算得出的經(jīng)度誤差方差值；圖5為本發(fā)明所述INS慣性導航與GPS導航以及 磁力計的導航算法計算得出的高度誤差值；圖6為本發(fā)明所述INS慣性導航與GPS導航以及 磁力計的導航算法計算得出的高度誤差方差值；圖7為本發(fā)明所述INS慣性導航與GPS導航 以及磁力計的導航算法計算得出的東向速度誤差值；圖8為本發(fā)明所述INS慣性導航與GPS 導航以及磁力計的導航算法計算得出的東向速度誤差方差值;圖9為本發(fā)明所述INS慣性導 航與GPS導航以及磁力計的導航算法計算得出的北向速度誤差值；圖10為本發(fā)明所述INS慣 性導航與GPS導航以及磁力計的導航算法計算得出的北向速度誤差方差值；圖11為本發(fā)明 所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的天向速度誤差值；圖12為本 發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法計算得出的天向速度誤差方差值； 從上述測試結(jié)果圖可以看出，本發(fā)明所述INS慣性導航與GPS導航以及磁力計的導航算法得 出的經(jīng)度、煒度、高度的誤差方差均能快速收斂至比較小的數(shù)值;對位置、速度等導航信息 也能實現(xiàn)濾平滑作用，不會產(chǎn)生大的跳變，系統(tǒng)的穩(wěn)定性較強。
[0153] 實施例一
[0154] 使用MATLAB對本發(fā)明所述用于四旋翼飛行器的控制方法進行仿真，設置滾轉(zhuǎn)、俯 仰和偏航角的初始值為某一非零角度，目標值均為零度，即飛機姿態(tài)保持水平，通過本發(fā)明 所述用于四旋翼飛行器的控制方法進行控制，在經(jīng)過一段時間后，三個姿態(tài)角均能趨近于 零度，使姿態(tài)保持水平。圖13為滾轉(zhuǎn)角的仿真效果圖；圖14為俯仰角的仿真效果圖；圖15為 偏航角的仿真效果圖；
[0155] 實施例二
[0156] 在三維坐標中，設置四旋翼飛行器的初始位置在(0,0,0，），目標期望的位置坐標 為（1，〇，1)，經(jīng)過反步法的控制，使飛機逼近坐標點（1，〇，1)。圖16為四旋翼飛行器在x軸，y 軸和z軸的位置仿真圖；圖17為四旋翼飛行器在三維空間中的軌跡圖。
[0157] 實施例三
[0158] 設置4個航點（0，1，1)，（1，1，1)，（1，0，1)，（0,0，1)，用本發(fā)明所述用于四旋翼飛行 器的控制方法控制四旋翼飛行器依次經(jīng)過這些航點，圖18為四旋翼飛行器的飛行軌跡圖。 [0159]實驗表明，用本發(fā)明所述用于四旋翼飛行器的控制方法對四旋翼飛行器的姿態(tài)角 和位置均能有效、快速地控制其收斂于目標值。
【主權項】
1. 用于四旋翼飛行器的控制方法，其特征在于包括以下步驟： 51、 利用INS慣性導航與GPS導航以及磁力計的導航算法計算得到四旋翼飛行器的導航 信息參數(shù)； 52、 將步驟S1得到的導航信息參數(shù)代入如下公式得到四旋翼飛行器的控制輸入量山、 U2、U3、U4;其中，Χ1= γ ,? =.f =?.，Χ3 = Θ，x4:=汐= ir3，.? =砂 =:多=?，θ是俯仰角，γ 是滾轉(zhuǎn)角，廬 是偏航角，Ωρ為轉(zhuǎn)速擾動，m為四旋翼飛行器的質(zhì)量，g為重力加速度，(^、(^(^、(^、(^、(^、(^、(^為大1表示四旋翼飛行器的機臂長度，Ιχχ，Iyy，Izz分別是四旋翼飛行器繞X軸，y軸，z軸的轉(zhuǎn)動 慣量，Jr為四旋翼飛行器的電機轉(zhuǎn)子的轉(zhuǎn)動慣量，21是俯仰角的目標值和通過步驟S1計算得 到實際值之差， Z2是俯仰角速度目標值和通過步驟S1計算得到實際值之差;Z3是滾轉(zhuǎn)角的目 標值和通過步驟S1計算得到實際值之差， Z4是滾轉(zhuǎn)角速度目標值和通過步驟S1計算得到實 際值之差;Z5是偏航角的目標值和通過步驟S1計算得到實際值之差，Z6是偏航角速度目標值 和通過步驟S1計算得到實際值之差； 27是2軸高度的目標值和通過步驟S1計算得到實際值 之差，Z8是Z軸高度方向上速度目標值和通過步驟S1計算得到實際值之差； 53、 將步驟S2計算得到的四旋翼飛行器的控制輸入量^、^、^、山輸入四旋翼飛行器的 控制系統(tǒng)進行飛行控制。2. 如權利要求1所述的用于四旋翼飛行器的控制方法，其特征在于:在步驟S1中，所述 INS慣性導航與GPS導航以及磁力計的導航算法的具體計算方法如下所述： A、將INS慣性導航系統(tǒng)的陀螺儀測得的四旋翼飛行器角速度參數(shù)6么,，代入 四元數(shù)微分方程求解得到四元數(shù)9〇^1，屯，陽；其中64,64,64為陀螺儀在四旋翼飛行 器自身坐標系下的測得的三個軸的角速度信息； 所述四元數(shù)微分方程為：B、 將步驟A中求解的qo，qi，q2，q3代入下式求解得到姿態(tài)矩陣0，根據(jù)下述與方向余弦的關系式計算得出四旋翼飛行器的INS慣性導航模塊姿態(tài)角θ、γ、P; C、 利用磁力計測得的偏航角P替換步驟B計算得到的偏航角 D、 將INS慣性導航系統(tǒng)的加速度計測得的加速度參數(shù)戶和步驟B中求解得到姿態(tài)矩陣0 代入下述微分方程中求解得到四旋翼飛行器在INS慣性導航坐標系下的東、北、天三個方向 上的速度信息VN VE VU，所述微分方程為：其中，Vn=[VN VE VU]'分別為INS慣性導航坐標系中東、北、天方向上的速度，為地球 自轉(zhuǎn)角速度，< 為四旋翼飛行器繞INS慣性導航坐標系各軸向的轉(zhuǎn)動角速率，gn為重力加速 度； E、 將步驟D計算得出的VN VE Vu分別代入下式求解得出四旋翼飛行器在INS慣性導航系 統(tǒng)中的位置信息，其中L為煒度，λ為經(jīng)度，h為高度，h = h(0)+JVudt，其中L(0)表示四旋翼飛行器初 '， 始位置的煒度值，λ(ο)表示四旋翼飛行器初始位置的經(jīng)度值，h(0)表示四旋翼飛行器距離 地球表面的初始高度。Rm表示地球子午圈上的曲率半徑，Rn表示煒度圈上的曲率半徑； F、 建立狀態(tài)方程< ⑴=6⑴⑴十G, (/)%⑴和觀測方程Z(t) =H(t)X〗(t)+V(t)， XKt)表示INS慣性導航系統(tǒng)在t時刻的誤差狀態(tài)，它是一個15維的向量，如下所示： 9vy ：Svz φχ ^ φζ 81 ^ Μ εχ c Vr ν^,δγχ,5Vy,5Vz^INS 慣性導航系統(tǒng)沿東、北、天方向上的速度誤差；Φχ，ΦΥ，Φζ為四旋翼飛行器的姿態(tài)角誤差;S ?，δλ，δ?!分別代表四旋翼飛行器所在煒度、經(jīng)度和高度誤差；εχ，εγ，εζ分別代表陀螺儀的隨 機漂移；1，1，1分別為加速度計的隨機漂移，是一個15 \15的矩陣；其中？〃(〇對應于61，6%，61，(^，(^，(})^1^\411這9個參數(shù)的1吣慣性導 航系統(tǒng)矩陣，其非零元素如下：Fs(t)為3"，5%，5￥2，(^，(^，(|)2況，3入，5]1這9個參數(shù)與陀螺儀及加速度計漂移之間的 變換矩陣，F(xiàn)M⑴為^士士^^^與陀螺儀及加速度計漂移對應的預對貫性導航系統(tǒng)矩陣^ 一個維數(shù)為6 X 6的對角線矩陣，表示如下： FM(t)=diag[-l/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz];其中，Tgx表示陀螺儀X軸的 誤差模型的時間常數(shù)，Tgy表示陀螺儀y軸的誤差模型的時間常數(shù)，Tgz表示陀螺儀z軸的誤差 模型的時間常數(shù)，T ax表示加速度計X軸誤差模型的時間常數(shù)，Tay表示加速度計y軸誤差模型 的時間常數(shù)，T az表示加速度計z軸誤差模型的時間常數(shù)； Gi(t)=diag[l 11......1 1]?5χ?5; Wi(t)是一個15維的向量，如下所示： Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio aii ai2 ai3 ai4 ai5]， ai a2 a3 a4 as a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系統(tǒng)過程噪聲序列； Z(t)為四旋翼飛行器在INS慣性導航中的位置速度信息與四旋翼飛行器在GPS導航系 統(tǒng)中的位置速度信息的差值，是一個6維向量， Ζ(?) = [δνχ+Ννχ δνγ+Ννγ δνζ+Ννζ (RM+h)3L+Ny (RM+h)cosL3A+Nx 3h+Nh]T，其中，Ννχ表不 GPS導航系統(tǒng)在x方向上的速度誤差，Nvy表示GPS導航系統(tǒng)在y方向上的速度誤差，Nvz表示 GPS導航系統(tǒng)在ζ方向上的速度誤差，Νχ表示GPS導航系統(tǒng)在X方向上的位置誤差，Ny表示GPS 導航系統(tǒng)在y方向上的位置誤差，Nh表示GPS導航系統(tǒng)在z方向上的位置誤差； G、 將上述得到的連續(xù)狀態(tài)方利Λ,(〇二dR, (/) + (?,(/)%⑴離散化后得到Xk = Φ??,??-iXk-ι+Wk-1，將上述得到的連續(xù)觀測方程Ζ (t) =H( t )X: (t) +V( t)離散化后得到Zk=HkXk+Vk; 其中I是單位矩陣，F(xiàn)是INS慣性導航系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，△ t是離散化后INS慣性導航 系統(tǒng)的采樣時間； H、 將四旋翼飛行器在INS慣性導航系統(tǒng)中的位置速度信息與四旋翼飛行器在GPS導航 系統(tǒng)中的位置速度信息作差得到Z(t)在k時刻的觀測信息ζ; I、 計算k時亥[|INS慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值之，之=之(ζ-J , 其中，之Η = Φ，之為在k -1時刻IN S慣性導航系統(tǒng)狀態(tài)的最優(yōu)估計值， 尸i =Φ…尸A-^INS慣性導航系統(tǒng)的噪聲矩陣， 其大小是由INS慣性導航元件的性能決定，+心/?，/ , Rk是系統(tǒng)測量噪聲的方差陣，其大小是由GPS接收機的性能決定； J、 將計算得到的值與四旋翼飛行器在INS慣性導航系統(tǒng)中的位置速度信息作差得 到最優(yōu)的導航參數(shù)； K、 重復步驟H-J，得到連續(xù)的四旋翼飛行器的導航信息參數(shù)。
【文檔編號】G05D1/08GK105929836SQ201610244268
【公開日】2016年9月7日
【申請日】2016年4月19日
【發(fā)明人】張瑜, 李詩揚
【申請人】成都翼比特自動化設備有限公司